/// NOTE: This is a generated file; do not edit.
#pragma once
#include <cnkalman/generated_header.h>

// clang-format off
static inline void LighthouseIMUPrediction(CnMat* out, const SurvivePose* _x1) {
	const FLT x0 = (*_x1).Rot[2] * (*_x1).Rot[2];
	const FLT x1 = (*_x1).Rot[1] * (*_x1).Rot[1];
	const FLT x2 = 1. / (x1 + ((*_x1).Rot[0] * (*_x1).Rot[0]) + ((*_x1).Rot[3] * (*_x1).Rot[3]) + x0);
	const FLT x3 = x2 * (*_x1).Rot[1];
	const FLT x4 = x2 * (*_x1).Rot[2];
	cnMatrixOptionalSet(out, 0, 0, 2 * ((-1 * x4 * (*_x1).Rot[0]) + (x3 * (*_x1).Rot[3])));
	cnMatrixOptionalSet(out, 1, 0, 2 * ((x4 * (*_x1).Rot[3]) + (x3 * (*_x1).Rot[0])));
	cnMatrixOptionalSet(out, 2, 0, 1 + (2 * ((-1 * x2 * x1) + (-1 * x0 * x2))));
}

// Jacobian of LighthouseIMUPrediction wrt [(*_x1).Pos[0], (*_x1).Pos[1], (*_x1).Pos[2], (*_x1).Rot[0], (*_x1).Rot[1], (*_x1).Rot[2], (*_x1).Rot[3]]
static inline void LighthouseIMUPrediction_jac_x1(CnMat* Hx, const SurvivePose* _x1) {
	const FLT x0 = (*_x1).Rot[0] * (*_x1).Rot[0];
	const FLT x1 = (*_x1).Rot[3] * (*_x1).Rot[3];
	const FLT x2 = (*_x1).Rot[2] * (*_x1).Rot[2];
	const FLT x3 = (*_x1).Rot[1] * (*_x1).Rot[1];
	const FLT x4 = x3 + x0 + x1 + x2;
	const FLT x5 = 1. / (x4 * x4);
	const FLT x6 = 2 * x5;
	const FLT x7 = x6 * (*_x1).Rot[2];
	const FLT x8 = x6 * (*_x1).Rot[1];
	const FLT x9 = x8 * (*_x1).Rot[3];
	const FLT x10 = -1 * x9 * (*_x1).Rot[0];
	const FLT x11 = 1. / x4;
	const FLT x12 = x11 * (*_x1).Rot[2];
	const FLT x13 = x8 * (*_x1).Rot[2] * (*_x1).Rot[0];
	const FLT x14 = x11 * (*_x1).Rot[3];
	const FLT x15 = x3 * x6;
	const FLT x16 = x2 * x6;
	const FLT x17 = -1 * x9 * (*_x1).Rot[2];
	const FLT x18 = x11 * (*_x1).Rot[0];
	const FLT x19 = x7 * (*_x1).Rot[3] * (*_x1).Rot[0];
	const FLT x20 = x11 * (*_x1).Rot[1];
	const FLT x21 = 4 * x5;
	const FLT x22 = x21 * (*_x1).Rot[0];
	const FLT x23 = x2 * x21;
	const FLT x24 = x3 * x21;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), 2 * ((-1 * x12) + (x0 * x7) + x10));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), 2 * ((-1 * x15 * (*_x1).Rot[3]) + x13 + x14));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), 2 * ((-1 * x18) + (x16 * (*_x1).Rot[0]) + x17));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), 2 * ((-1 * x1 * x8) + x19 + x20));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[0])/sizeof(FLT), 2 * ((-1 * x0 * x8) + (-1 * x19) + x20));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[1])/sizeof(FLT), 2 * (x17 + (-1 * x15 * (*_x1).Rot[0]) + x18));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[2])/sizeof(FLT), 2 * (x14 + (-1 * x16 * (*_x1).Rot[3]) + (-1 * x13)));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[3])/sizeof(FLT), 2 * (x12 + (-1 * x1 * x7) + x10));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[0])/sizeof(FLT), (x3 * x22) + (x2 * x22));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[1])/sizeof(FLT), (x23 * (*_x1).Rot[1]) + (-4 * x20) + (x21 * ((*_x1).Rot[1] * (*_x1).Rot[1] * (*_x1).Rot[1])));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[2])/sizeof(FLT), (-4 * x12) + (x21 * ((*_x1).Rot[2] * (*_x1).Rot[2] * (*_x1).Rot[2])) + (x24 * (*_x1).Rot[2]));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[3])/sizeof(FLT), (x24 * (*_x1).Rot[3]) + (x23 * (*_x1).Rot[3]));
}

// Full version Jacobian of LighthouseIMUPrediction wrt [(*_x1).Pos[0], (*_x1).Pos[1], (*_x1).Pos[2], (*_x1).Rot[0], (*_x1).Rot[1], (*_x1).Rot[2], (*_x1).Rot[3]]

static inline void LighthouseIMUPrediction_jac_x1_with_hx(CnMat* Hx, CnMat* hx, const SurvivePose* _x1) {
    if(hx != 0) { 
        LighthouseIMUPrediction(hx, _x1);
    }
    if(Hx != 0) { 
        LighthouseIMUPrediction_jac_x1(Hx, _x1);
    }
}
static inline void LighthouseErrorIMUPrediction(CnMat* out, const SurvivePose* _x1, const SurviveAxisAnglePose* error_state) {
	const FLT x0 = 0.5 * (*_x1).Rot[3];
	const FLT x1 = 0.5 * (*_x1).Rot[2];
	const FLT x2 = 0.5 * (*error_state).AxisAngleRot[0];
	const FLT x3 = (*_x1).Rot[1] + (x2 * (*_x1).Rot[0]) + (-1 * x0 * (*error_state).AxisAngleRot[1]) + (x1 * (*error_state).AxisAngleRot[2]);
	const FLT x4 = 0.5 * (*_x1).Rot[0];
	const FLT x5 = 0.5 * (*_x1).Rot[1];
	const FLT x6 = (x5 * (*error_state).AxisAngleRot[1]) + (x4 * (*error_state).AxisAngleRot[2]) + (*_x1).Rot[3] + (-1 * x2 * (*_x1).Rot[2]);
	const FLT x7 = x6 * x6;
	const FLT x8 = (-1 * x5 * (*error_state).AxisAngleRot[2]) + (*_x1).Rot[2] + (x4 * (*error_state).AxisAngleRot[1]) + (x2 * (*_x1).Rot[3]);
	const FLT x9 = x8 * x8;
	const FLT x10 = (-1 * x2 * (*_x1).Rot[1]) + (*_x1).Rot[0] + (-1 * x0 * (*error_state).AxisAngleRot[2]) + (-1 * x1 * (*error_state).AxisAngleRot[1]);
	const FLT x11 = x10 * x10;
	const FLT x12 = x3 * x3;
	const FLT x13 = 1. / (x11 + x12 + x7 + x9);
	const FLT x14 = x9 * x13;
	const FLT x15 = x13 * x12;
	const FLT x16 = 1. / (x15 + (x13 * x11) + (x7 * x13) + x14);
	const FLT x17 = x13 * x16;
	const FLT x18 = x6 * x17;
	const FLT x19 = x10 * x17;
	cnMatrixOptionalSet(out, 0, 0, 2 * ((-1 * x8 * x19) + (x3 * x18)));
	cnMatrixOptionalSet(out, 1, 0, 2 * ((x8 * x18) + (x3 * x19)));
	cnMatrixOptionalSet(out, 2, 0, 1 + (2 * ((-1 * x15 * x16) + (-1 * x14 * x16))));
}

// Jacobian of LighthouseErrorIMUPrediction wrt [(*_x1).Pos[0], (*_x1).Pos[1], (*_x1).Pos[2], (*_x1).Rot[0], (*_x1).Rot[1], (*_x1).Rot[2], (*_x1).Rot[3]]
static inline void LighthouseErrorIMUPrediction_jac_x1(CnMat* Hx, const SurvivePose* _x1, const SurviveAxisAnglePose* error_state) {
	const FLT x0 = 0.5 * (*_x1).Rot[2];
	const FLT x1 = 0.5 * (*_x1).Rot[0];
	const FLT x2 = 0.5 * (*error_state).AxisAngleRot[1];
	const FLT x3 = (x2 * (*_x1).Rot[1]) + (x1 * (*error_state).AxisAngleRot[2]) + (*_x1).Rot[3] + (-1 * x0 * (*error_state).AxisAngleRot[0]);
	const FLT x4 = x3 * x3;
	const FLT x5 = 0.5 * (*_x1).Rot[3];
	const FLT x6 = 0.5 * (*_x1).Rot[1];
	const FLT x7 = (-1 * x6 * (*error_state).AxisAngleRot[2]) + (x1 * (*error_state).AxisAngleRot[1]) + (*_x1).Rot[2] + (x5 * (*error_state).AxisAngleRot[0]);
	const FLT x8 = x7 * x7;
	const FLT x9 = (-1 * x6 * (*error_state).AxisAngleRot[0]) + (*_x1).Rot[0] + (-1 * x5 * (*error_state).AxisAngleRot[2]) + (-1 * x2 * (*_x1).Rot[2]);
	const FLT x10 = x9 * x9;
	const FLT x11 = (*_x1).Rot[1] + (x1 * (*error_state).AxisAngleRot[0]) + (-1 * x5 * (*error_state).AxisAngleRot[1]) + (x0 * (*error_state).AxisAngleRot[2]);
	const FLT x12 = x11 * x11;
	const FLT x13 = x10 + x12 + x4 + x8;
	const FLT x14 = 1. / x13;
	const FLT x15 = x8 * x14;
	const FLT x16 = x14 * x12;
	const FLT x17 = (x14 * x10) + (x4 * x14) + x16 + x15;
	const FLT x18 = 1. / (x17 * x17);
	const FLT x19 = 1. / (x13 * x13);
	const FLT x20 = 1.0 * x11;
	const FLT x21 = x20 * (*error_state).AxisAngleRot[0];
	const FLT x22 = 2 * x9;
	const FLT x23 = 1.0 * x3;
	const FLT x24 = x23 * (*error_state).AxisAngleRot[2];
	const FLT x25 = 1.0 * x7;
	const FLT x26 = x25 * (*error_state).AxisAngleRot[1];
	const FLT x27 = x26 + x24 + x21 + x22;
	const FLT x28 = x27 * x19;
	const FLT x29 = x28 * x12;
	const FLT x30 = x4 * x19;
	const FLT x31 = x8 * x28;
	const FLT x32 = ((-1 * x31) + (-1 * x30 * x27) + (x21 * x14) + (x26 * x14) + (-1 * x28 * x10) + (-1 * x29) + (x24 * x14) + (x22 * x14)) * x18;
	const FLT x33 = x9 * x14;
	const FLT x34 = x7 * x33;
	const FLT x35 = 1. / x17;
	const FLT x36 = x35 * x14;
	const FLT x37 = x7 * x36;
	const FLT x38 = x36 * x11;
	const FLT x39 = 0.5 * (*error_state).AxisAngleRot[2];
	const FLT x40 = x38 * x39;
	const FLT x41 = x9 * x35;
	const FLT x42 = x41 * x28;
	const FLT x43 = x3 * x11;
	const FLT x44 = x43 * x14;
	const FLT x45 = x35 * x28;
	const FLT x46 = x9 * x36;
	const FLT x47 = x3 * x36;
	const FLT x48 = 0.5 * (*error_state).AxisAngleRot[0];
	const FLT x49 = (x47 * x48) + (-1 * x2 * x46);
	const FLT x50 = 2 * x11;
	const FLT x51 = x23 * (*error_state).AxisAngleRot[1];
	const FLT x52 = 1.0 * x9;
	const FLT x53 = x52 * (*error_state).AxisAngleRot[0];
	const FLT x54 = x25 * (*error_state).AxisAngleRot[2];
	const FLT x55 = (-1 * x54) + (-1 * x53) + x50 + x51;
	const FLT x56 = x55 * x19;
	const FLT x57 = x8 * x56;
	const FLT x58 = x56 * x12;
	const FLT x59 = (-1 * x53 * x14) + (x50 * x14) + (-1 * x55 * x30) + (-1 * x56 * x10) + (x51 * x14) + (-1 * x54 * x14) + (-1 * x57) + (-1 * x58);
	const FLT x60 = x59 * x18;
	const FLT x61 = x48 * x37;
	const FLT x62 = x56 * x41;
	const FLT x63 = x2 * x38;
	const FLT x64 = x56 * x35;
	const FLT x65 = (x46 * x39) + x47;
	const FLT x66 = x20 * (*error_state).AxisAngleRot[2];
	const FLT x67 = x23 * (*error_state).AxisAngleRot[0];
	const FLT x68 = x52 * (*error_state).AxisAngleRot[1];
	const FLT x69 = 2 * x7;
	const FLT x70 = x69 + (-1 * x68) + x66 + (-1 * x67);
	const FLT x71 = x70 * x19;
	const FLT x72 = x71 * x41;
	const FLT x73 = x47 * x39;
	const FLT x74 = x71 * x35;
	const FLT x75 = x71 * x12;
	const FLT x76 = x8 * x71;
	const FLT x77 = (-1 * x68 * x14) + (-1 * x76) + (x66 * x14) + (x69 * x14) + (-1 * x75) + (-1 * x67 * x14) + (-1 * x71 * x10) + (-1 * x70 * x30);
	const FLT x78 = x77 * x18;
	const FLT x79 = (x2 * x37) + (-1 * x48 * x38);
	const FLT x80 = x20 * (*error_state).AxisAngleRot[1];
	const FLT x81 = x52 * (*error_state).AxisAngleRot[2];
	const FLT x82 = 2 * x3;
	const FLT x83 = x25 * (*error_state).AxisAngleRot[0];
	const FLT x84 = x83 + x82 + (-1 * x80) + (-1 * x81);
	const FLT x85 = x84 * x19;
	const FLT x86 = x85 * x35;
	const FLT x87 = x9 * x86;
	const FLT x88 = x46 * x48;
	const FLT x89 = x2 * x47;
	const FLT x90 = x85 * x12;
	const FLT x91 = x8 * x85;
	const FLT x92 = ((x83 * x14) + (-1 * x91) + (x82 * x14) + (-1 * x80 * x14) + (-1 * x90) + (-1 * x85 * x10) + (-1 * x81 * x14) + (-1 * x84 * x30)) * x18;
	const FLT x93 = (x37 * x39) + x38;
	const FLT x94 = x3 * x7;
	const FLT x95 = x94 * x14;
	const FLT x96 = x33 * x11;
	const FLT x97 = 2 * x35;
	const FLT x98 = 2.0 * x38;
	const FLT x99 = 2.0 * x37;
	const FLT x100 = 2 * x32;
	const FLT x101 = 2 * x18;
	const FLT x102 = x59 * x101;
	const FLT x103 = x77 * x101;
	const FLT x104 = 2 * x92;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[0])/sizeof(FLT), 2 * ((-1 * x43 * x45) + x49 + (x32 * x34) + (x7 * x42) + (-1 * x37) + x40 + (-1 * x44 * x32)));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[1])/sizeof(FLT), 2 * (x61 + (x7 * x62) + (-1 * x64 * x43) + (x60 * x34) + (-1 * x60 * x44) + x65 + x63));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[2])/sizeof(FLT), 2 * ((-1 * x46) + (-1 * x74 * x43) + x79 + x73 + (x7 * x72) + (x78 * x34) + (-1 * x78 * x44)));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurvivePose, Rot[3])/sizeof(FLT), 2 * (x93 + (-1 * x86 * x43) + (x7 * x87) + (-1 * x89) + (-1 * x88) + (x92 * x34) + (-1 * x92 * x44)));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[0])/sizeof(FLT), 2 * (x93 + (-1 * x42 * x11) + x88 + (-1 * x94 * x45) + (-1 * x95 * x32) + x89 + (-1 * x96 * x32)));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[1])/sizeof(FLT), 2 * ((-1 * x62 * x11) + x46 + (-1 * x64 * x94) + x79 + (-1 * x73) + (-1 * x60 * x95) + (-1 * x60 * x96)));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[2])/sizeof(FLT), 2 * ((-1 * x61) + x65 + (-1 * x63) + (-1 * x78 * x95) + (-1 * x74 * x94) + (-1 * x72 * x11) + (-1 * x78 * x96)));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurvivePose, Rot[3])/sizeof(FLT), 2 * ((-1 * x87 * x11) + x49 + (-1 * x92 * x95) + (-1 * x86 * x94) + (-1 * x92 * x96) + (-1 * x40) + x37));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[0])/sizeof(FLT), (x97 * x31) + (-1 * x99 * (*error_state).AxisAngleRot[1]) + (x15 * x100) + (x97 * x29) + (-1 * x98 * (*error_state).AxisAngleRot[0]) + (x16 * x100));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[1])/sizeof(FLT), (x99 * (*error_state).AxisAngleRot[2]) + (-4 * x38) + (x57 * x97) + (x58 * x97) + (x15 * x102) + (x16 * x102));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[2])/sizeof(FLT), (x15 * x103) + (x76 * x97) + (x16 * x103) + (x75 * x97) + (-4 * x37) + (-1 * x98 * (*error_state).AxisAngleRot[2]));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurvivePose, Rot[3])/sizeof(FLT), (x98 * (*error_state).AxisAngleRot[1]) + (x15 * x104) + (-1 * x99 * (*error_state).AxisAngleRot[0]) + (x91 * x97) + (x16 * x104) + (x90 * x97));
}

// Full version Jacobian of LighthouseErrorIMUPrediction wrt [(*_x1).Pos[0], (*_x1).Pos[1], (*_x1).Pos[2], (*_x1).Rot[0], (*_x1).Rot[1], (*_x1).Rot[2], (*_x1).Rot[3]]

static inline void LighthouseErrorIMUPrediction_jac_x1_with_hx(CnMat* Hx, CnMat* hx, const SurvivePose* _x1, const SurviveAxisAnglePose* error_state) {
    if(hx != 0) { 
        LighthouseErrorIMUPrediction(hx, _x1, error_state);
    }
    if(Hx != 0) { 
        LighthouseErrorIMUPrediction_jac_x1(Hx, _x1, error_state);
    }
}
// Jacobian of LighthouseErrorIMUPrediction wrt [(*error_state).AxisAngleRot[0], (*error_state).AxisAngleRot[1], (*error_state).AxisAngleRot[2], (*error_state).Pos[0], (*error_state).Pos[1], (*error_state).Pos[2]]
static inline void LighthouseErrorIMUPrediction_jac_error_state(CnMat* Hx, const SurvivePose* _x1, const SurviveAxisAnglePose* error_state) {
	const FLT x0 = 0.5 * (*error_state).AxisAngleRot[1];
	const FLT x1 = 0.5 * (*error_state).AxisAngleRot[2];
	const FLT x2 = 0.5 * (*_x1).Rot[0];
	const FLT x3 = (*_x1).Rot[1] + (x2 * (*error_state).AxisAngleRot[0]) + (-1 * x0 * (*_x1).Rot[3]) + (x1 * (*_x1).Rot[2]);
	const FLT x4 = x3 * x3;
	const FLT x5 = 0.5 * (*error_state).AxisAngleRot[0];
	const FLT x6 = (-1 * x5 * (*_x1).Rot[1]) + (-1 * x1 * (*_x1).Rot[3]) + (*_x1).Rot[0] + (-1 * x0 * (*_x1).Rot[2]);
	const FLT x7 = 1.0 * (*_x1).Rot[1];
	const FLT x8 = x6 * x7;
	const FLT x9 = (x0 * (*_x1).Rot[1]) + (x2 * (*error_state).AxisAngleRot[2]) + (*_x1).Rot[3] + (-1 * x5 * (*_x1).Rot[2]);
	const FLT x10 = 1.0 * x9;
	const FLT x11 = x10 * (*_x1).Rot[2];
	const FLT x12 = 1.0 * x3;
	const FLT x13 = x12 * (*_x1).Rot[0];
	const FLT x14 = (*_x1).Rot[2] + (-1 * x1 * (*_x1).Rot[1]) + (x0 * (*_x1).Rot[0]) + (x5 * (*_x1).Rot[3]);
	const FLT x15 = 1.0 * x14;
	const FLT x16 = x15 * (*_x1).Rot[3];
	const FLT x17 = x16 + x13 + (-1 * x8) + (-1 * x11);
	const FLT x18 = x9 * x9;
	const FLT x19 = x14 * x14;
	const FLT x20 = x6 * x6;
	const FLT x21 = x20 + x4 + x18 + x19;
	const FLT x22 = 1. / (x21 * x21);
	const FLT x23 = x22 * x17;
	const FLT x24 = x4 * x23;
	const FLT x25 = 1. / x21;
	const FLT x26 = x22 * x20;
	const FLT x27 = x23 * x19;
	const FLT x28 = (-1 * x27) + (-1 * x25 * x11) + (x25 * x13) + (-1 * x26 * x17) + (-1 * x24) + (x25 * x16) + (-1 * x8 * x25) + (-1 * x23 * x18);
	const FLT x29 = x25 * x19;
	const FLT x30 = x4 * x25;
	const FLT x31 = x30 + (x25 * x20) + (x25 * x18) + x29;
	const FLT x32 = 1. / (x31 * x31);
	const FLT x33 = x32 * x25;
	const FLT x34 = x33 * x28;
	const FLT x35 = x6 * x14;
	const FLT x36 = 1. / x31;
	const FLT x37 = x36 * x23;
	const FLT x38 = x3 * x9;
	const FLT x39 = x36 * x25;
	const FLT x40 = x2 * x39;
	const FLT x41 = 0.5 * x39;
	const FLT x42 = x6 * x41;
	const FLT x43 = x41 * (*_x1).Rot[2];
	const FLT x44 = x41 * x14;
	const FLT x45 = (x44 * (*_x1).Rot[1]) + (-1 * x3 * x43) + (x9 * x40) + (-1 * x42 * (*_x1).Rot[3]);
	const FLT x46 = 1.0 * x6;
	const FLT x47 = x46 * (*_x1).Rot[2];
	const FLT x48 = x7 * x9;
	const FLT x49 = x12 * (*_x1).Rot[3];
	const FLT x50 = x15 * (*_x1).Rot[0];
	const FLT x51 = x50 + (-1 * x49) + (-1 * x47) + x48;
	const FLT x52 = x51 * x22;
	const FLT x53 = x52 * x36;
	const FLT x54 = x3 * x41;
	const FLT x55 = x54 * (*_x1).Rot[1];
	const FLT x56 = x4 * x52;
	const FLT x57 = x52 * x19;
	const FLT x58 = (-1 * x57) + (-1 * x47 * x25) + (x50 * x25) + (-1 * x56) + (-1 * x51 * x26) + (-1 * x49 * x25) + (-1 * x52 * x18) + (x48 * x25);
	const FLT x59 = x58 * x33;
	const FLT x60 = x43 * x14;
	const FLT x61 = x9 * x41;
	const FLT x62 = x61 * (*_x1).Rot[3];
	const FLT x63 = x6 * x40;
	const FLT x64 = x12 * (*_x1).Rot[2];
	const FLT x65 = x46 * (*_x1).Rot[3];
	const FLT x66 = x10 * (*_x1).Rot[0];
	const FLT x67 = x7 * x14;
	const FLT x68 = (-1 * x67) + x66 + x64 + (-1 * x65);
	const FLT x69 = x68 * x22;
	const FLT x70 = x69 * x36;
	const FLT x71 = x4 * x69;
	const FLT x72 = x69 * x19;
	const FLT x73 = (-1 * x72) + (-1 * x71) + (-1 * x67 * x25) + (-1 * x69 * x18) + (-1 * x68 * x26) + (x64 * x25) + (-1 * x65 * x25) + (x66 * x25);
	const FLT x74 = x73 * x33;
	const FLT x75 = x9 * x14;
	const FLT x76 = x3 * x6;
	const FLT x77 = 2.0 * x39;
	const FLT x78 = x3 * x77;
	const FLT x79 = 2 * x32;
	const FLT x80 = x79 * x28;
	const FLT x81 = 2 * x36;
	const FLT x82 = x77 * x14;
	const FLT x83 = x79 * x58;
	const FLT x84 = x73 * x79;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), 2 * (x45 + (-1 * x34 * x38) + (x34 * x35) + (-1 * x38 * x37) + (x35 * x37)));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), 2 * ((-1 * x53 * x38) + x55 + x60 + (-1 * x63) + (x53 * x35) + (-1 * x59 * x38) + (x59 * x35) + (-1 * x62)));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), 2 * ((-1 * x70 * x38) + (-1 * x74 * x38) + (x74 * x35) + (x9 * x43) + (x44 * (*_x1).Rot[3]) + (x42 * (*_x1).Rot[1]) + (x3 * x40) + (x70 * x35)));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), 2 * ((-1 * x76 * x37) + x63 + (-1 * x55) + (-1 * x75 * x34) + (-1 * x76 * x34) + (-1 * x75 * x37) + x62 + (-1 * x60)));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), 2 * (x45 + (-1 * x76 * x59) + (-1 * x75 * x53) + (-1 * x76 * x53) + (-1 * x75 * x59)));
	cnMatrixOptionalSet(Hx, 1, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), 2 * ((-1 * x61 * (*_x1).Rot[1]) + (-1 * x70 * x76) + (-1 * x54 * (*_x1).Rot[3]) + (-1 * x75 * x74) + (-1 * x70 * x75) + (x40 * x14) + (x6 * x43) + (-1 * x74 * x76)));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurviveAxisAnglePose, AxisAngleRot[0])/sizeof(FLT), (x80 * x30) + (-1 * x78 * (*_x1).Rot[0]) + (x80 * x29) + (x81 * x24) + (x81 * x27) + (-1 * x82 * (*_x1).Rot[3]));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurviveAxisAnglePose, AxisAngleRot[1])/sizeof(FLT), (x83 * x29) + (x78 * (*_x1).Rot[3]) + (x81 * x57) + (x83 * x30) + (-1 * x82 * (*_x1).Rot[0]) + (x81 * x56));
	cnMatrixOptionalSet(Hx, 2, offsetof(SurviveAxisAnglePose, AxisAngleRot[2])/sizeof(FLT), (x84 * x29) + (x84 * x30) + (x82 * (*_x1).Rot[1]) + (x81 * x72) + (-1 * x78 * (*_x1).Rot[2]) + (x81 * x71));
}

// Full version Jacobian of LighthouseErrorIMUPrediction wrt [(*error_state).AxisAngleRot[0], (*error_state).AxisAngleRot[1], (*error_state).AxisAngleRot[2], (*error_state).Pos[0], (*error_state).Pos[1], (*error_state).Pos[2]]

static inline void LighthouseErrorIMUPrediction_jac_error_state_with_hx(CnMat* Hx, CnMat* hx, const SurvivePose* _x1, const SurviveAxisAnglePose* error_state) {
    if(hx != 0) { 
        LighthouseErrorIMUPrediction(hx, _x1, error_state);
    }
    if(Hx != 0) { 
        LighthouseErrorIMUPrediction_jac_error_state(Hx, _x1, error_state);
    }
}
static inline void SurviveLighthouseKalmanModelToErrorModel(SurviveLighthouseKalmanErrorModel* out, const SurviveLighthouseKalmanModel* _x1, const SurviveLighthouseKalmanModel* _x0) {
	const FLT x0 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]);
	const FLT x1 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]);
	const FLT x2 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]);
	const FLT x3 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]);
	const FLT x4 = x0 * x0;
	out->Lighthouse.Pos[0]=(*_x1).Lighthouse.Pos[0] + (-1 * (*_x0).Lighthouse.Pos[0]);
	out->Lighthouse.Pos[1]=(*_x1).Lighthouse.Pos[1] + (-1 * (*_x0).Lighthouse.Pos[1]);
	out->Lighthouse.Pos[2]=(*_x1).Lighthouse.Pos[2] + (-1 * (*_x0).Lighthouse.Pos[2]);
	out->Lighthouse.AxisAngleRot[0]=atan2(2 * ((x2 * x3) + (x0 * x1)), 1 + (-2 * ((x3 * x3) + x4)));
	out->Lighthouse.AxisAngleRot[1]=asin(2 * ((x0 * x2) + (-1 * x1 * x3)));
	out->Lighthouse.AxisAngleRot[2]=atan2(2 * ((x2 * x1) + (x0 * x3)), 1 + (-2 * (x4 + (x1 * x1))));
	out->BSD0.phase=(*_x1).BSD0.phase + (-1 * (*_x0).BSD0.phase);
	out->BSD0.tilt=(*_x1).BSD0.tilt + (-1 * (*_x0).BSD0.tilt);
	out->BSD0.curve=(*_x1).BSD0.curve + (-1 * (*_x0).BSD0.curve);
	out->BSD0.gibpha=(*_x1).BSD0.gibpha + (-1 * (*_x0).BSD0.gibpha);
	out->BSD0.gibmag=(*_x1).BSD0.gibmag + (-1 * (*_x0).BSD0.gibmag);
	out->BSD0.ogeephase=(*_x1).BSD0.ogeephase + (-1 * (*_x0).BSD0.ogeephase);
	out->BSD0.ogeemag=(*_x1).BSD0.ogeemag + (-1 * (*_x0).BSD0.ogeemag);
	out->BSD1.phase=(*_x1).BSD1.phase + (-1 * (*_x0).BSD1.phase);
	out->BSD1.tilt=(*_x1).BSD1.tilt + (-1 * (*_x0).BSD1.tilt);
	out->BSD1.curve=(*_x1).BSD1.curve + (-1 * (*_x0).BSD1.curve);
	out->BSD1.gibpha=(*_x1).BSD1.gibpha + (-1 * (*_x0).BSD1.gibpha);
	out->BSD1.gibmag=(*_x1).BSD1.gibmag + (-1 * (*_x0).BSD1.gibmag);
	out->BSD1.ogeephase=(*_x1).BSD1.ogeephase + (-1 * (*_x0).BSD1.ogeephase);
	out->BSD1.ogeemag=(*_x1).BSD1.ogeemag + (-1 * (*_x0).BSD1.ogeemag);
}

// Jacobian of SurviveLighthouseKalmanModelToErrorModel wrt [(*_x1).Lighthouse.Pos[0], (*_x1).Lighthouse.Pos[1], (*_x1).Lighthouse.Pos[2], (*_x1).Lighthouse.Rot[0], (*_x1).Lighthouse.Rot[1], (*_x1).Lighthouse.Rot[2], (*_x1).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveLighthouseKalmanModelToErrorModel_jac_x1(CnMat* Hx, const SurviveLighthouseKalmanModel* _x1, const SurviveLighthouseKalmanModel* _x0) {
	const FLT x0 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]);
	const FLT x1 = x0 * (*_x0).Lighthouse.Rot[3];
	const FLT x2 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]);
	const FLT x3 = x2 * (*_x0).Lighthouse.Rot[2];
	const FLT x4 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]);
	const FLT x5 = x4 * (*_x0).Lighthouse.Rot[0];
	const FLT x6 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]);
	const FLT x7 = x6 * (*_x0).Lighthouse.Rot[1];
	const FLT x8 = (-1 * x7) + x5;
	const FLT x9 = x0 * x0;
	const FLT x10 = 1 + (-2 * ((x4 * x4) + x9));
	const FLT x11 = 2 * (1. / x10);
	const FLT x12 = x0 * (*_x0).Lighthouse.Rot[2];
	const FLT x13 = 4 * x12;
	const FLT x14 = x4 * (*_x0).Lighthouse.Rot[1];
	const FLT x15 = x10 * x10;
	const FLT x16 = (x4 * x6) + (x0 * x2);
	const FLT x17 = 2 * (1. / x15) * x16;
	const FLT x18 = x15 * (1. / (x15 + (4 * (x16 * x16))));
	const FLT x19 = x2 * (*_x0).Lighthouse.Rot[3];
	const FLT x20 = x14 + (x6 * (*_x0).Lighthouse.Rot[0]);
	const FLT x21 = x20 + x12 + (-1 * x19);
	const FLT x22 = 4 * x1;
	const FLT x23 = x4 * (*_x0).Lighthouse.Rot[2];
	const FLT x24 = x6 * (*_x0).Lighthouse.Rot[3];
	const FLT x25 = x0 * (*_x0).Lighthouse.Rot[1];
	const FLT x26 = x2 * (*_x0).Lighthouse.Rot[0];
	const FLT x27 = x26 + (-1 * x25);
	const FLT x28 = x0 * (*_x0).Lighthouse.Rot[0];
	const FLT x29 = -4 * x28;
	const FLT x30 = x4 * (*_x0).Lighthouse.Rot[3];
	const FLT x31 = x6 * (*_x0).Lighthouse.Rot[2];
	const FLT x32 = x2 * (*_x0).Lighthouse.Rot[1];
	const FLT x33 = x28 + x32;
	const FLT x34 = x33 + x30 + (-1 * x31);
	const FLT x35 = -4 * x25;
	const FLT x36 = 2 * (1. / sqrt(1 + (-4 * (((x0 * x6) + (-1 * x2 * x4)) * ((x0 * x6) + (-1 * x2 * x4))))));
	const FLT x37 = (-1 * x23) + (-1 * x24);
	const FLT x38 = x3 + x1;
	const FLT x39 = 1 + (-2 * (x9 + (x2 * x2)));
	const FLT x40 = 2 * (1. / x39);
	const FLT x41 = x39 * x39;
	const FLT x42 = (x2 * x6) + (x0 * x4);
	const FLT x43 = 2 * (1. / x41) * x42;
	const FLT x44 = x41 * (1. / (x41 + (4 * (x42 * x42))));
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x18 * ((-1 * ((4 * x14) + x13) * x17) + ((x8 + (-1 * x1) + (-1 * x3)) * x11)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x18 * ((-1 * x17 * ((-4 * x5) + x22)) + (x21 * x11)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x18 * ((-1 * ((-4 * x30) + x29) * x17) + (x11 * (x23 + x27 + x24))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x18 * ((-1 * ((4 * x23) + x35) * x17) + (x34 * x11)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x34 * x36);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x36 * (x37 + x25 + (-1 * x26)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x36 * x21);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x36 * (x38 + x7 + (-1 * x5)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), ((-1 * (x13 + (4 * x19)) * x43) + ((x37 + x27) * x40)) * x44);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (-4 * x3))) + (x40 * (x31 + x33 + (-1 * x30)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (4 * x32)) * x43) + (x40 * (x38 + x8))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (-4 * x26)) * x43) + (x40 * (x20 + (-1 * x12) + x19))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeemag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeemag)/sizeof(FLT), 1);
}

// Full version Jacobian of SurviveLighthouseKalmanModelToErrorModel wrt [(*_x1).Lighthouse.Pos[0], (*_x1).Lighthouse.Pos[1], (*_x1).Lighthouse.Pos[2], (*_x1).Lighthouse.Rot[0], (*_x1).Lighthouse.Rot[1], (*_x1).Lighthouse.Rot[2], (*_x1).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
// Jacobian of SurviveLighthouseKalmanModelToErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveLighthouseKalmanModelToErrorModel_jac_x0(CnMat* Hx, const SurviveLighthouseKalmanModel* _x1, const SurviveLighthouseKalmanModel* _x0) {
	const FLT x0 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]);
	const FLT x1 = x0 * (*_x1).Lighthouse.Rot[3];
	const FLT x2 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]);
	const FLT x3 = x2 * (*_x1).Lighthouse.Rot[0];
	const FLT x4 = x3 + x1;
	const FLT x5 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]);
	const FLT x6 = x5 * (*_x1).Lighthouse.Rot[1];
	const FLT x7 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]);
	const FLT x8 = x7 * (*_x1).Lighthouse.Rot[2];
	const FLT x9 = x8 + x6;
	const FLT x10 = x0 * x0;
	const FLT x11 = 1 + (-2 * ((x2 * x2) + x10));
	const FLT x12 = 2 * (1. / x11);
	const FLT x13 = x0 * (*_x1).Lighthouse.Rot[2];
	const FLT x14 = -4 * x13;
	const FLT x15 = x2 * (*_x1).Lighthouse.Rot[1];
	const FLT x16 = x11 * x11;
	const FLT x17 = (x2 * x5) + (x0 * x7);
	const FLT x18 = 2 * x17 * (1. / x16);
	const FLT x19 = x16 * (1. / (x16 + (4 * (x17 * x17))));
	const FLT x20 = x7 * (*_x1).Lighthouse.Rot[3];
	const FLT x21 = x20 + (-1 * x5 * (*_x1).Lighthouse.Rot[0]);
	const FLT x22 = -4 * x1;
	const FLT x23 = x5 * (*_x1).Lighthouse.Rot[3];
	const FLT x24 = x7 * (*_x1).Lighthouse.Rot[0];
	const FLT x25 = x2 * (*_x1).Lighthouse.Rot[2];
	const FLT x26 = x0 * (*_x1).Lighthouse.Rot[1];
	const FLT x27 = x26 + x25;
	const FLT x28 = x0 * (*_x1).Lighthouse.Rot[0];
	const FLT x29 = 4 * x28;
	const FLT x30 = x2 * (*_x1).Lighthouse.Rot[3];
	const FLT x31 = x30 + (-1 * x28);
	const FLT x32 = x5 * (*_x1).Lighthouse.Rot[2];
	const FLT x33 = x7 * (*_x1).Lighthouse.Rot[1];
	const FLT x34 = (-1 * x33) + x32;
	const FLT x35 = 4 * x26;
	const FLT x36 = 2 * (1. / sqrt(1 + (-4 * (((x0 * x5) + (-1 * x2 * x7)) * ((x0 * x5) + (-1 * x2 * x7))))));
	const FLT x37 = x23 + x27 + x24;
	const FLT x38 = x21 + x13 + (-1 * x15);
	const FLT x39 = 1 + (-2 * (x10 + (x7 * x7)));
	const FLT x40 = 2 * (1. / x39);
	const FLT x41 = x39 * x39;
	const FLT x42 = (x5 * x7) + (x0 * x2);
	const FLT x43 = 2 * (1. / x41) * x42;
	const FLT x44 = x41 * (1. / (x41 + (4 * (x42 * x42))));
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), ((-1 * ((-4 * x15) + x14) * x18) + ((x9 + x4) * x12)) * x19);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x19 * ((-1 * x18 * ((4 * x3) + x22)) + (x12 * (x21 + (-1 * x13) + x15))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x19 * ((-1 * ((4 * x30) + x29) * x18) + (x12 * ((-1 * x23) + x27 + (-1 * x24)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), ((-1 * ((-4 * x25) + x35) * x18) + ((x34 + x31) * x12)) * x19);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x36 * (x34 + (-1 * x30) + x28));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x36 * x37);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x36 * x38);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (x4 + (-1 * x6) + (-1 * x8)) * x36);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x44 * ((-1 * (x14 + (-4 * x20)) * x43) + (x40 * x37)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (4 * x8))) + (x40 * (x31 + x33 + (-1 * x32)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (-4 * x33)) * x43) + ((x9 + (-1 * x1) + (-1 * x3)) * x40)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (4 * x24)) * x43) + (x40 * x38)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.phase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.tilt)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.curve)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibpha)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibmag)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeephase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeemag)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.phase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.tilt)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.curve)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibpha)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibmag)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeephase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeemag)/sizeof(FLT), -1);
}

// Full version Jacobian of SurviveLighthouseKalmanModelToErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveLighthouseKalmanModelAddErrorModel(SurviveLighthouseKalmanModel* out, const SurviveLighthouseKalmanModel* _x0, const SurviveLighthouseKalmanErrorModel* error_state) {
	const FLT x0 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[2];
	const FLT x1 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[1];
	const FLT x2 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[0];
	const FLT x3 = (*_x0).Lighthouse.Rot[0] + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (-1 * x0 * (*_x0).Lighthouse.Rot[3]) + (-1 * x1 * (*_x0).Lighthouse.Rot[2]);
	const FLT x4 = (x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (-1 * x2 * (*_x0).Lighthouse.Rot[2]) + (x0 * (*_x0).Lighthouse.Rot[0]);
	const FLT x5 = (x1 * (*_x0).Lighthouse.Rot[0]) + (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (x2 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2];
	const FLT x6 = (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[1] + (x2 * (*_x0).Lighthouse.Rot[0]) + (x0 * (*_x0).Lighthouse.Rot[2]);
	const FLT x7 = 1. / sqrt((x6 * x6) + (x5 * x5) + (x3 * x3) + (x4 * x4));
	out->Lighthouse.Pos[0]=(*_x0).Lighthouse.Pos[0] + (*error_state).Lighthouse.Pos[0];
	out->Lighthouse.Pos[1]=(*_x0).Lighthouse.Pos[1] + (*error_state).Lighthouse.Pos[1];
	out->Lighthouse.Pos[2]=(*_x0).Lighthouse.Pos[2] + (*error_state).Lighthouse.Pos[2];
	out->Lighthouse.Rot[0]=x3 * x7;
	out->Lighthouse.Rot[1]=x6 * x7;
	out->Lighthouse.Rot[2]=x5 * x7;
	out->Lighthouse.Rot[3]=x4 * x7;
	out->BSD0.phase=(*error_state).BSD0.phase + (*_x0).BSD0.phase;
	out->BSD0.tilt=(*error_state).BSD0.tilt + (*_x0).BSD0.tilt;
	out->BSD0.curve=(*error_state).BSD0.curve + (*_x0).BSD0.curve;
	out->BSD0.gibpha=(*error_state).BSD0.gibpha + (*_x0).BSD0.gibpha;
	out->BSD0.gibmag=(*error_state).BSD0.gibmag + (*_x0).BSD0.gibmag;
	out->BSD0.ogeephase=(*error_state).BSD0.ogeephase + (*_x0).BSD0.ogeephase;
	out->BSD0.ogeemag=(*error_state).BSD0.ogeemag + (*_x0).BSD0.ogeemag;
	out->BSD1.phase=(*error_state).BSD1.phase + (*_x0).BSD1.phase;
	out->BSD1.tilt=(*error_state).BSD1.tilt + (*_x0).BSD1.tilt;
	out->BSD1.curve=(*error_state).BSD1.curve + (*_x0).BSD1.curve;
	out->BSD1.gibpha=(*error_state).BSD1.gibpha + (*_x0).BSD1.gibpha;
	out->BSD1.gibmag=(*error_state).BSD1.gibmag + (*_x0).BSD1.gibmag;
	out->BSD1.ogeephase=(*error_state).BSD1.ogeephase + (*_x0).BSD1.ogeephase;
	out->BSD1.ogeemag=(*error_state).BSD1.ogeemag + (*_x0).BSD1.ogeemag;
}

// Jacobian of SurviveLighthouseKalmanModelAddErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveLighthouseKalmanModelAddErrorModel_jac_x0(CnMat* Hx, const SurviveLighthouseKalmanModel* _x0, const SurviveLighthouseKalmanErrorModel* error_state) {
	const FLT x0 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[2];
	const FLT x1 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[1];
	const FLT x2 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[0];
	const FLT x3 = (*_x0).Lighthouse.Rot[0] + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (-1 * x0 * (*_x0).Lighthouse.Rot[3]) + (-1 * x1 * (*_x0).Lighthouse.Rot[2]);
	const FLT x4 = (x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (-1 * x2 * (*_x0).Lighthouse.Rot[2]) + (x0 * (*_x0).Lighthouse.Rot[0]);
	const FLT x5 = (x1 * (*_x0).Lighthouse.Rot[0]) + (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (x2 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2];
	const FLT x6 = (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[1] + (x2 * (*_x0).Lighthouse.Rot[0]) + (x0 * (*_x0).Lighthouse.Rot[2]);
	const FLT x7 = (x6 * x6) + (x5 * x5) + (x3 * x3) + (x4 * x4);
	const FLT x8 = 1. / sqrt(x7);
	const FLT x9 = 1.0 * x5;
	const FLT x10 = 1.0 * x6;
	const FLT x11 = 1.0 * (*error_state).Lighthouse.AxisAngleRot[2];
	const FLT x12 = 1.0/2.0 * (1. / (x7 * sqrt(x7)));
	const FLT x13 = x12 * ((x4 * x11) + (x10 * (*error_state).Lighthouse.AxisAngleRot[0]) + (x9 * (*error_state).Lighthouse.AxisAngleRot[1]) + (2 * x3));
	const FLT x14 = 0.5 * x8;
	const FLT x15 = x14 * (*error_state).Lighthouse.AxisAngleRot[0];
	const FLT x16 = -1 * x15;
	const FLT x17 = 1.0 * x3;
	const FLT x18 = 1.0 * x4;
	const FLT x19 = (x18 * (*error_state).Lighthouse.AxisAngleRot[1]) + (-1 * x17 * (*error_state).Lighthouse.AxisAngleRot[0]) + (2 * x6) + (-1 * x5 * x11);
	const FLT x20 = x12 * x19;
	const FLT x21 = x14 * (*error_state).Lighthouse.AxisAngleRot[1];
	const FLT x22 = -1 * x21;
	const FLT x23 = (-1 * x17 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x6 * x11) + (-1 * x18 * (*error_state).Lighthouse.AxisAngleRot[0]) + (2 * x5);
	const FLT x24 = x23 * x12;
	const FLT x25 = x14 * (*error_state).Lighthouse.AxisAngleRot[2];
	const FLT x26 = -1 * x25;
	const FLT x27 = x12 * ((2 * x4) + (-1 * x10 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x9 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x3 * x11));
	const FLT x28 = x6 * x12;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x3 * x13) + x8);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x3 * x20) + x16);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x3 * x24) + x22);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x3 * x27) + x26);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x6 * x13) + x15);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x28 * x19) + x8);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x23 * x28) + x25);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x6 * x27) + x22);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x5 * x13) + x21);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x5 * x20) + x26);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x5 * x24) + x8);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x5 * x27) + x15);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x4 * x13) + x25);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x4 * x20) + x21);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x4 * x24) + x16);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x4 * x27) + x8);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD0.ogeemag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanModel, BSD1.ogeemag)/sizeof(FLT), 1);
}

// Full version Jacobian of SurviveLighthouseKalmanModelAddErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
// Jacobian of SurviveLighthouseKalmanModelAddErrorModel wrt [(*error_state).Lighthouse.AxisAngleRot[0], (*error_state).Lighthouse.AxisAngleRot[1], (*error_state).Lighthouse.AxisAngleRot[2], (*error_state).Lighthouse.Pos[0], (*error_state).Lighthouse.Pos[1], (*error_state).Lighthouse.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveLighthouseKalmanModelAddErrorModel_jac_error_state(CnMat* Hx, const SurviveLighthouseKalmanModel* _x0, const SurviveLighthouseKalmanErrorModel* error_state) {
	const FLT x0 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[2];
	const FLT x1 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[1];
	const FLT x2 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[0];
	const FLT x3 = (*_x0).Lighthouse.Rot[0] + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (-1 * x0 * (*_x0).Lighthouse.Rot[3]) + (-1 * x1 * (*_x0).Lighthouse.Rot[2]);
	const FLT x4 = 0.5 * (*_x0).Lighthouse.Rot[0];
	const FLT x5 = (x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (-1 * x2 * (*_x0).Lighthouse.Rot[2]) + (x4 * (*error_state).Lighthouse.AxisAngleRot[2]);
	const FLT x6 = (x4 * (*error_state).Lighthouse.AxisAngleRot[1]) + (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (x2 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2];
	const FLT x7 = (x4 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[1] + (x0 * (*_x0).Lighthouse.Rot[2]);
	const FLT x8 = (x7 * x7) + (x6 * x6) + (x3 * x3) + (x5 * x5);
	const FLT x9 = 1. / sqrt(x8);
	const FLT x10 = 0.5 * x9;
	const FLT x11 = x10 * (*_x0).Lighthouse.Rot[1];
	const FLT x12 = -1 * x11;
	const FLT x13 = 1.0 * x7;
	const FLT x14 = 1.0 * x6;
	const FLT x15 = 1.0 * x3;
	const FLT x16 = 1.0 * (*_x0).Lighthouse.Rot[2];
	const FLT x17 = (-1 * x5 * x16) + (-1 * x15 * (*_x0).Lighthouse.Rot[1]) + (x13 * (*_x0).Lighthouse.Rot[0]) + (x14 * (*_x0).Lighthouse.Rot[3]);
	const FLT x18 = 1.0/2.0 * (1. / (x8 * sqrt(x8)));
	const FLT x19 = x3 * x18;
	const FLT x20 = x10 * (*_x0).Lighthouse.Rot[2];
	const FLT x21 = -1 * x20;
	const FLT x22 = 1.0 * x5;
	const FLT x23 = (x22 * (*_x0).Lighthouse.Rot[1]) + (x14 * (*_x0).Lighthouse.Rot[0]) + (-1 * x13 * (*_x0).Lighthouse.Rot[3]) + (-1 * x3 * x16);
	const FLT x24 = x10 * (*_x0).Lighthouse.Rot[3];
	const FLT x25 = -1 * x24;
	const FLT x26 = (x22 * (*_x0).Lighthouse.Rot[0]) + (-1 * x15 * (*_x0).Lighthouse.Rot[3]) + (x7 * x16) + (-1 * x14 * (*_x0).Lighthouse.Rot[1]);
	const FLT x27 = x4 * x9;
	const FLT x28 = x7 * x18;
	const FLT x29 = x18 * x17;
	const FLT x30 = x23 * x18;
	const FLT x31 = x26 * x18;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x19 * x17) + x12);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x23 * x19) + x21);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x26 * x19) + x25);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x28 * x17) + x27);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x23 * x28) + x25);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x28 * x26) + x20);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x6 * x29) + x24);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x6 * x30) + x27);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x6 * x31) + x12);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x5 * x29) + x21);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x5 * x30) + x11);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x5 * x31) + x27);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveLighthouseKalmanModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveLighthouseKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), 1);
}

// Full version Jacobian of SurviveLighthouseKalmanModelAddErrorModel wrt [(*error_state).Lighthouse.AxisAngleRot[0], (*error_state).Lighthouse.AxisAngleRot[1], (*error_state).Lighthouse.AxisAngleRot[2], (*error_state).Lighthouse.Pos[0], (*error_state).Lighthouse.Pos[1], (*error_state).Lighthouse.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanModelToErrorModel(SurviveJointKalmanErrorModel* out, const SurviveJointKalmanModel* _x1, const SurviveJointKalmanModel* _x0) {
	const FLT x0 = ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[1]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[3]);
	const FLT x1 = (-1 * (*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[3]);
	const FLT x2 = (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[3]) + (-1 * (*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[2]);
	const FLT x3 = ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[3]) + ((*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[2]);
	const FLT x4 = x0 * x0;
	const FLT x5 = ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[2]);
	const FLT x6 = (-1 * (*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[1]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[3]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[0]);
	const FLT x7 = ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[2]);
	const FLT x8 = ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[0]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[3]);
	const FLT x9 = x5 * x5;
	const FLT x10 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]);
	const FLT x11 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]);
	const FLT x12 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]);
	const FLT x13 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]);
	const FLT x14 = x10 * x10;
	out->Object.Pose.Pos[0]=(*_x1).Object.Pose.Pos[0] + (-1 * (*_x0).Object.Pose.Pos[0]);
	out->Object.Pose.Pos[1]=(*_x1).Object.Pose.Pos[1] + (-1 * (*_x0).Object.Pose.Pos[1]);
	out->Object.Pose.Pos[2]=(*_x1).Object.Pose.Pos[2] + (-1 * (*_x0).Object.Pose.Pos[2]);
	out->Object.Pose.AxisAngleRot[0]=atan2(2 * ((x2 * x3) + (x0 * x1)), 1 + (-2 * ((x2 * x2) + x4)));
	out->Object.Pose.AxisAngleRot[1]=asin(2 * ((x0 * x3) + (-1 * x2 * x1)));
	out->Object.Pose.AxisAngleRot[2]=atan2(2 * ((x1 * x3) + (x0 * x2)), 1 + (-2 * (x4 + (x1 * x1))));
	out->Object.Velocity.Pos[0]=(*_x1).Object.Velocity.Pos[0] + (-1 * (*_x0).Object.Velocity.Pos[0]);
	out->Object.Velocity.Pos[1]=(*_x1).Object.Velocity.Pos[1] + (-1 * (*_x0).Object.Velocity.Pos[1]);
	out->Object.Velocity.Pos[2]=(*_x1).Object.Velocity.Pos[2] + (-1 * (*_x0).Object.Velocity.Pos[2]);
	out->Object.Velocity.AxisAngleRot[0]=(*_x1).Object.Velocity.AxisAngleRot[0] + (-1 * (*_x0).Object.Velocity.AxisAngleRot[0]);
	out->Object.Velocity.AxisAngleRot[1]=(*_x1).Object.Velocity.AxisAngleRot[1] + (-1 * (*_x0).Object.Velocity.AxisAngleRot[1]);
	out->Object.Velocity.AxisAngleRot[2]=(*_x1).Object.Velocity.AxisAngleRot[2] + (-1 * (*_x0).Object.Velocity.AxisAngleRot[2]);
	out->Object.Acc[0]=(*_x1).Object.Acc[0] + (-1 * (*_x0).Object.Acc[0]);
	out->Object.Acc[1]=(*_x1).Object.Acc[1] + (-1 * (*_x0).Object.Acc[1]);
	out->Object.Acc[2]=(*_x1).Object.Acc[2] + (-1 * (*_x0).Object.Acc[2]);
	out->Object.IMUBias.AccScale[0]=(*_x1).Object.IMUBias.AccScale[0] + (-1 * (*_x0).Object.IMUBias.AccScale[0]);
	out->Object.IMUBias.AccScale[1]=(*_x1).Object.IMUBias.AccScale[1] + (-1 * (*_x0).Object.IMUBias.AccScale[1]);
	out->Object.IMUBias.AccScale[2]=(*_x1).Object.IMUBias.AccScale[2] + (-1 * (*_x0).Object.IMUBias.AccScale[2]);
	out->Object.IMUBias.IMUCorrection[0]=atan2(2 * ((x8 * x7) + (x6 * x5)), 1 + (-2 * ((x7 * x7) + x9)));
	out->Object.IMUBias.IMUCorrection[1]=asin(2 * ((x5 * x8) + (-1 * x6 * x7)));
	out->Object.IMUBias.IMUCorrection[2]=atan2(2 * ((x6 * x8) + (x5 * x7)), 1 + (-2 * (x9 + (x6 * x6))));
	out->Object.IMUBias.AccBias[0]=(*_x1).Object.IMUBias.AccBias[0] + (-1 * (*_x0).Object.IMUBias.AccBias[0]);
	out->Object.IMUBias.AccBias[1]=(*_x1).Object.IMUBias.AccBias[1] + (-1 * (*_x0).Object.IMUBias.AccBias[1]);
	out->Object.IMUBias.AccBias[2]=(*_x1).Object.IMUBias.AccBias[2] + (-1 * (*_x0).Object.IMUBias.AccBias[2]);
	out->Object.IMUBias.GyroBias[0]=(*_x1).Object.IMUBias.GyroBias[0] + (-1 * (*_x0).Object.IMUBias.GyroBias[0]);
	out->Object.IMUBias.GyroBias[1]=(*_x1).Object.IMUBias.GyroBias[1] + (-1 * (*_x0).Object.IMUBias.GyroBias[1]);
	out->Object.IMUBias.GyroBias[2]=(*_x1).Object.IMUBias.GyroBias[2] + (-1 * (*_x0).Object.IMUBias.GyroBias[2]);
	out->Lighthouse.Pos[0]=(*_x1).Lighthouse.Pos[0] + (-1 * (*_x0).Lighthouse.Pos[0]);
	out->Lighthouse.Pos[1]=(*_x1).Lighthouse.Pos[1] + (-1 * (*_x0).Lighthouse.Pos[1]);
	out->Lighthouse.Pos[2]=(*_x1).Lighthouse.Pos[2] + (-1 * (*_x0).Lighthouse.Pos[2]);
	out->Lighthouse.AxisAngleRot[0]=atan2(2 * ((x13 * x12) + (x11 * x10)), 1 + (-2 * ((x13 * x13) + x14)));
	out->Lighthouse.AxisAngleRot[1]=asin(2 * ((x12 * x10) + (-1 * x13 * x11)));
	out->Lighthouse.AxisAngleRot[2]=atan2(2 * ((x12 * x11) + (x13 * x10)), 1 + (-2 * (x14 + (x11 * x11))));
	out->BSD0.phase=(*_x1).BSD0.phase + (-1 * (*_x0).BSD0.phase);
	out->BSD0.tilt=(*_x1).BSD0.tilt + (-1 * (*_x0).BSD0.tilt);
	out->BSD0.curve=(*_x1).BSD0.curve + (-1 * (*_x0).BSD0.curve);
	out->BSD0.gibpha=(*_x1).BSD0.gibpha + (-1 * (*_x0).BSD0.gibpha);
	out->BSD0.gibmag=(*_x1).BSD0.gibmag + (-1 * (*_x0).BSD0.gibmag);
	out->BSD0.ogeephase=(*_x1).BSD0.ogeephase + (-1 * (*_x0).BSD0.ogeephase);
	out->BSD0.ogeemag=(*_x1).BSD0.ogeemag + (-1 * (*_x0).BSD0.ogeemag);
	out->BSD1.phase=(*_x1).BSD1.phase + (-1 * (*_x0).BSD1.phase);
	out->BSD1.tilt=(*_x1).BSD1.tilt + (-1 * (*_x0).BSD1.tilt);
	out->BSD1.curve=(*_x1).BSD1.curve + (-1 * (*_x0).BSD1.curve);
	out->BSD1.gibpha=(*_x1).BSD1.gibpha + (-1 * (*_x0).BSD1.gibpha);
	out->BSD1.gibmag=(*_x1).BSD1.gibmag + (-1 * (*_x0).BSD1.gibmag);
	out->BSD1.ogeephase=(*_x1).BSD1.ogeephase + (-1 * (*_x0).BSD1.ogeephase);
	out->BSD1.ogeemag=(*_x1).BSD1.ogeemag + (-1 * (*_x0).BSD1.ogeemag);
}

// Jacobian of SurviveJointKalmanModelToErrorModel wrt [(*_x1).Lighthouse.Pos[0], (*_x1).Lighthouse.Pos[1], (*_x1).Lighthouse.Pos[2], (*_x1).Lighthouse.Rot[0], (*_x1).Lighthouse.Rot[1], (*_x1).Lighthouse.Rot[2], (*_x1).Lighthouse.Rot[3], (*_x1).Object.Acc[0], (*_x1).Object.Acc[1], (*_x1).Object.Acc[2], (*_x1).Object.IMUBias.AccBias[0], (*_x1).Object.IMUBias.AccBias[1], (*_x1).Object.IMUBias.AccBias[2], (*_x1).Object.IMUBias.AccScale[0], (*_x1).Object.IMUBias.AccScale[1], (*_x1).Object.IMUBias.AccScale[2], (*_x1).Object.IMUBias.GyroBias[0], (*_x1).Object.IMUBias.GyroBias[1], (*_x1).Object.IMUBias.GyroBias[2], (*_x1).Object.IMUBias.IMUCorrection[0], (*_x1).Object.IMUBias.IMUCorrection[1], (*_x1).Object.IMUBias.IMUCorrection[2], (*_x1).Object.IMUBias.IMUCorrection[3], (*_x1).Object.Pose.Pos[0], (*_x1).Object.Pose.Pos[1], (*_x1).Object.Pose.Pos[2], (*_x1).Object.Pose.Rot[0], (*_x1).Object.Pose.Rot[1], (*_x1).Object.Pose.Rot[2], (*_x1).Object.Pose.Rot[3], (*_x1).Object.Velocity.AxisAngleRot[0], (*_x1).Object.Velocity.AxisAngleRot[1], (*_x1).Object.Velocity.AxisAngleRot[2], (*_x1).Object.Velocity.Pos[0], (*_x1).Object.Velocity.Pos[1], (*_x1).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanModelToErrorModel_jac_x1(CnMat* Hx, const SurviveJointKalmanModel* _x1, const SurviveJointKalmanModel* _x0) {
	const FLT x0 = ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[1]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[3]);
	const FLT x1 = x0 * (*_x0).Object.Pose.Rot[3];
	const FLT x2 = (-1 * (*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[3]);
	const FLT x3 = x2 * (*_x0).Object.Pose.Rot[2];
	const FLT x4 = ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[3]) + ((*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[2]);
	const FLT x5 = x4 * (*_x0).Object.Pose.Rot[1];
	const FLT x6 = (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[3]) + (-1 * (*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[2]);
	const FLT x7 = x6 * (*_x0).Object.Pose.Rot[0];
	const FLT x8 = x7 + (-1 * x5);
	const FLT x9 = x0 * x0;
	const FLT x10 = 1 + (-2 * ((x6 * x6) + x9));
	const FLT x11 = 2 * (1. / x10);
	const FLT x12 = x0 * (*_x0).Object.Pose.Rot[2];
	const FLT x13 = 4 * x12;
	const FLT x14 = x6 * (*_x0).Object.Pose.Rot[1];
	const FLT x15 = x10 * x10;
	const FLT x16 = (x4 * x6) + (x0 * x2);
	const FLT x17 = 2 * (1. / x15) * x16;
	const FLT x18 = x15 * (1. / (x15 + (4 * (x16 * x16))));
	const FLT x19 = x2 * (*_x0).Object.Pose.Rot[3];
	const FLT x20 = x14 + (x4 * (*_x0).Object.Pose.Rot[0]);
	const FLT x21 = x20 + x12 + (-1 * x19);
	const FLT x22 = 4 * x1;
	const FLT x23 = x4 * (*_x0).Object.Pose.Rot[3];
	const FLT x24 = x6 * (*_x0).Object.Pose.Rot[2];
	const FLT x25 = x0 * (*_x0).Object.Pose.Rot[1];
	const FLT x26 = x2 * (*_x0).Object.Pose.Rot[0];
	const FLT x27 = x26 + (-1 * x25);
	const FLT x28 = x0 * (*_x0).Object.Pose.Rot[0];
	const FLT x29 = -4 * x28;
	const FLT x30 = x6 * (*_x0).Object.Pose.Rot[3];
	const FLT x31 = x4 * (*_x0).Object.Pose.Rot[2];
	const FLT x32 = x2 * (*_x0).Object.Pose.Rot[1];
	const FLT x33 = x28 + x32;
	const FLT x34 = (-1 * x31) + x33 + x30;
	const FLT x35 = -4 * x25;
	const FLT x36 = 2 * (1. / sqrt(1 + (-4 * (((x0 * x4) + (-1 * x2 * x6)) * ((x0 * x4) + (-1 * x2 * x6))))));
	const FLT x37 = (-1 * x24) + (-1 * x23);
	const FLT x38 = x1 + x3;
	const FLT x39 = 1 + (-2 * (x9 + (x2 * x2)));
	const FLT x40 = 2 * (1. / x39);
	const FLT x41 = x39 * x39;
	const FLT x42 = (x2 * x4) + (x0 * x6);
	const FLT x43 = 2 * (1. / x41) * x42;
	const FLT x44 = x41 * (1. / (x41 + (4 * (x42 * x42))));
	const FLT x45 = ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[2]);
	const FLT x46 = x45 * (*_x0).Object.IMUBias.IMUCorrection[3];
	const FLT x47 = (-1 * (*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[1]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[3]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[0]);
	const FLT x48 = x47 * (*_x0).Object.IMUBias.IMUCorrection[2];
	const FLT x49 = ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[0]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[3]);
	const FLT x50 = x49 * (*_x0).Object.IMUBias.IMUCorrection[1];
	const FLT x51 = ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[2]);
	const FLT x52 = x51 * (*_x0).Object.IMUBias.IMUCorrection[0];
	const FLT x53 = x52 + (-1 * x50);
	const FLT x54 = x45 * x45;
	const FLT x55 = 1 + (-2 * ((x51 * x51) + x54));
	const FLT x56 = 2 * (1. / x55);
	const FLT x57 = x45 * (*_x0).Object.IMUBias.IMUCorrection[2];
	const FLT x58 = 4 * x57;
	const FLT x59 = x51 * (*_x0).Object.IMUBias.IMUCorrection[1];
	const FLT x60 = x55 * x55;
	const FLT x61 = (x51 * x49) + (x45 * x47);
	const FLT x62 = 2 * (1. / x60) * x61;
	const FLT x63 = x60 * (1. / (x60 + (4 * (x61 * x61))));
	const FLT x64 = x47 * (*_x0).Object.IMUBias.IMUCorrection[3];
	const FLT x65 = x59 + (x49 * (*_x0).Object.IMUBias.IMUCorrection[0]);
	const FLT x66 = x65 + x57 + (-1 * x64);
	const FLT x67 = 4 * x46;
	const FLT x68 = x51 * (*_x0).Object.IMUBias.IMUCorrection[2];
	const FLT x69 = x49 * (*_x0).Object.IMUBias.IMUCorrection[3];
	const FLT x70 = x45 * (*_x0).Object.IMUBias.IMUCorrection[1];
	const FLT x71 = x47 * (*_x0).Object.IMUBias.IMUCorrection[0];
	const FLT x72 = x71 + (-1 * x70);
	const FLT x73 = x45 * (*_x0).Object.IMUBias.IMUCorrection[0];
	const FLT x74 = -4 * x73;
	const FLT x75 = x51 * (*_x0).Object.IMUBias.IMUCorrection[3];
	const FLT x76 = x49 * (*_x0).Object.IMUBias.IMUCorrection[2];
	const FLT x77 = x47 * (*_x0).Object.IMUBias.IMUCorrection[1];
	const FLT x78 = x77 + x73;
	const FLT x79 = x78 + x75 + (-1 * x76);
	const FLT x80 = -4 * x70;
	const FLT x81 = 2 * (1. / sqrt(1 + (-4 * (((x45 * x49) + (-1 * x51 * x47)) * ((x45 * x49) + (-1 * x51 * x47))))));
	const FLT x82 = (-1 * x69) + (-1 * x68);
	const FLT x83 = x46 + x48;
	const FLT x84 = 1 + (-2 * (x54 + (x47 * x47)));
	const FLT x85 = 2 * (1. / x84);
	const FLT x86 = x84 * x84;
	const FLT x87 = (x47 * x49) + (x51 * x45);
	const FLT x88 = 2 * (1. / x86) * x87;
	const FLT x89 = x86 * (1. / (x86 + (4 * (x87 * x87))));
	const FLT x90 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]);
	const FLT x91 = x90 * (*_x0).Lighthouse.Rot[3];
	const FLT x92 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]);
	const FLT x93 = x92 * (*_x0).Lighthouse.Rot[2];
	const FLT x94 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]);
	const FLT x95 = x94 * (*_x0).Lighthouse.Rot[0];
	const FLT x96 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]);
	const FLT x97 = x96 * (*_x0).Lighthouse.Rot[1];
	const FLT x98 = (-1 * x97) + x95;
	const FLT x99 = x90 * x90;
	const FLT x100 = 1 + (-2 * ((x94 * x94) + x99));
	const FLT x101 = 2 * (1. / x100);
	const FLT x102 = x90 * (*_x0).Lighthouse.Rot[2];
	const FLT x103 = 4 * x102;
	const FLT x104 = x94 * (*_x0).Lighthouse.Rot[1];
	const FLT x105 = x100 * x100;
	const FLT x106 = (x96 * x94) + (x92 * x90);
	const FLT x107 = 2 * (1. / x105) * x106;
	const FLT x108 = x105 * (1. / (x105 + (4 * (x106 * x106))));
	const FLT x109 = x92 * (*_x0).Lighthouse.Rot[3];
	const FLT x110 = x104 + (x96 * (*_x0).Lighthouse.Rot[0]);
	const FLT x111 = x102 + x110 + (-1 * x109);
	const FLT x112 = 4 * x91;
	const FLT x113 = x94 * (*_x0).Lighthouse.Rot[2];
	const FLT x114 = x96 * (*_x0).Lighthouse.Rot[3];
	const FLT x115 = x90 * (*_x0).Lighthouse.Rot[1];
	const FLT x116 = x92 * (*_x0).Lighthouse.Rot[0];
	const FLT x117 = x116 + (-1 * x115);
	const FLT x118 = x90 * (*_x0).Lighthouse.Rot[0];
	const FLT x119 = -4 * x118;
	const FLT x120 = x94 * (*_x0).Lighthouse.Rot[3];
	const FLT x121 = x96 * (*_x0).Lighthouse.Rot[2];
	const FLT x122 = x92 * (*_x0).Lighthouse.Rot[1];
	const FLT x123 = x118 + x122;
	const FLT x124 = x123 + x120 + (-1 * x121);
	const FLT x125 = -4 * x115;
	const FLT x126 = 2 * (1. / sqrt(1 + (-4 * (((x90 * x96) + (-1 * x92 * x94)) * ((x90 * x96) + (-1 * x92 * x94))))));
	const FLT x127 = (-1 * x113) + (-1 * x114);
	const FLT x128 = x91 + x93;
	const FLT x129 = 1 + (-2 * (x99 + (x92 * x92)));
	const FLT x130 = 2 * (1. / x129);
	const FLT x131 = x129 * x129;
	const FLT x132 = (x92 * x96) + (x90 * x94);
	const FLT x133 = 2 * (1. / x131) * x132;
	const FLT x134 = x131 * (1. / (x131 + (4 * (x132 * x132))));
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x18 * ((-1 * ((4 * x14) + x13) * x17) + ((x8 + (-1 * x1) + (-1 * x3)) * x11)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x18 * ((-1 * x17 * ((-4 * x7) + x22)) + (x21 * x11)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x18 * ((-1 * ((-4 * x30) + x29) * x17) + (x11 * (x23 + x27 + x24))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x18 * ((-1 * ((4 * x24) + x35) * x17) + (x34 * x11)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x34 * x36);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x36 * (x37 + x25 + (-1 * x26)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x36 * x21);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x36 * (x38 + x5 + (-1 * x7)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), ((-1 * (x13 + (4 * x19)) * x43) + ((x37 + x27) * x40)) * x44);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (-4 * x3))) + (x40 * (x31 + x33 + (-1 * x30)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (4 * x32)) * x43) + (x40 * (x38 + x8))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (-4 * x26)) * x43) + (x40 * (x20 + x19 + (-1 * x12)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x63 * ((-1 * ((4 * x59) + x58) * x62) + (x56 * (x53 + (-1 * x46) + (-1 * x48)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x63 * ((-1 * ((-4 * x52) + x67) * x62) + (x66 * x56)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x63 * ((-1 * ((-4 * x75) + x74) * x62) + (x56 * (x72 + x68 + x69))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x63 * ((-1 * ((4 * x68) + x80) * x62) + (x79 * x56)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x81 * x79);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x81 * (x82 + x70 + (-1 * x71)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x81 * x66);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x81 * (x50 + x83 + (-1 * x52)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x89 * ((-1 * (x58 + (4 * x64)) * x88) + ((x72 + x82) * x85)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x89 * ((-1 * (x67 + (-4 * x48)) * x88) + (x85 * (x78 + x76 + (-1 * x75)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x89 * ((-1 * (x74 + (4 * x77)) * x88) + ((x83 + x53) * x85)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x89 * ((-1 * (x80 + (-4 * x71)) * x88) + (x85 * (x65 + x64 + (-1 * x57)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x108 * ((-1 * ((4 * x104) + x103) * x107) + ((x98 + (-1 * x91) + (-1 * x93)) * x101)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x108 * ((-1 * x107 * ((-4 * x95) + x112)) + (x101 * x111)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x108 * ((-1 * ((-4 * x120) + x119) * x107) + (x101 * (x117 + x113 + x114))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x108 * ((-1 * ((4 * x113) + x125) * x107) + (x101 * x124)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x124 * x126);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x126 * (x127 + x115 + (-1 * x116)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x111 * x126);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x126 * (x128 + x97 + (-1 * x95)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), ((-1 * (x103 + (4 * x109)) * x133) + ((x117 + x127) * x130)) * x134);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x134 * ((-1 * x133 * (x112 + (-4 * x93))) + (x130 * (x121 + x123 + (-1 * x120)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x134 * ((-1 * (x119 + (4 * x122)) * x133) + (x130 * (x128 + x98))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x134 * ((-1 * (x125 + (-4 * x116)) * x133) + (x130 * ((-1 * x102) + x110 + x109))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), 1);
}

// Full version Jacobian of SurviveJointKalmanModelToErrorModel wrt [(*_x1).Lighthouse.Pos[0], (*_x1).Lighthouse.Pos[1], (*_x1).Lighthouse.Pos[2], (*_x1).Lighthouse.Rot[0], (*_x1).Lighthouse.Rot[1], (*_x1).Lighthouse.Rot[2], (*_x1).Lighthouse.Rot[3], (*_x1).Object.Acc[0], (*_x1).Object.Acc[1], (*_x1).Object.Acc[2], (*_x1).Object.IMUBias.AccBias[0], (*_x1).Object.IMUBias.AccBias[1], (*_x1).Object.IMUBias.AccBias[2], (*_x1).Object.IMUBias.AccScale[0], (*_x1).Object.IMUBias.AccScale[1], (*_x1).Object.IMUBias.AccScale[2], (*_x1).Object.IMUBias.GyroBias[0], (*_x1).Object.IMUBias.GyroBias[1], (*_x1).Object.IMUBias.GyroBias[2], (*_x1).Object.IMUBias.IMUCorrection[0], (*_x1).Object.IMUBias.IMUCorrection[1], (*_x1).Object.IMUBias.IMUCorrection[2], (*_x1).Object.IMUBias.IMUCorrection[3], (*_x1).Object.Pose.Pos[0], (*_x1).Object.Pose.Pos[1], (*_x1).Object.Pose.Pos[2], (*_x1).Object.Pose.Rot[0], (*_x1).Object.Pose.Rot[1], (*_x1).Object.Pose.Rot[2], (*_x1).Object.Pose.Rot[3], (*_x1).Object.Velocity.AxisAngleRot[0], (*_x1).Object.Velocity.AxisAngleRot[1], (*_x1).Object.Velocity.AxisAngleRot[2], (*_x1).Object.Velocity.Pos[0], (*_x1).Object.Velocity.Pos[1], (*_x1).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
// Jacobian of SurviveJointKalmanModelToErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanModelToErrorModel_jac_x0(CnMat* Hx, const SurviveJointKalmanModel* _x1, const SurviveJointKalmanModel* _x0) {
	const FLT x0 = ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[1]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[3]);
	const FLT x1 = x0 * (*_x1).Object.Pose.Rot[3];
	const FLT x2 = (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[3]) + (-1 * (*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[2]);
	const FLT x3 = x2 * (*_x1).Object.Pose.Rot[0];
	const FLT x4 = x3 + x1;
	const FLT x5 = ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[3]) + ((*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[0]) + ((*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[2]);
	const FLT x6 = x5 * (*_x1).Object.Pose.Rot[1];
	const FLT x7 = (-1 * (*_x1).Object.Pose.Rot[2] * (*_x0).Object.Pose.Rot[1]) + ((*_x1).Object.Pose.Rot[1] * (*_x0).Object.Pose.Rot[2]) + ((*_x1).Object.Pose.Rot[3] * (*_x0).Object.Pose.Rot[0]) + (-1 * (*_x1).Object.Pose.Rot[0] * (*_x0).Object.Pose.Rot[3]);
	const FLT x8 = x7 * (*_x1).Object.Pose.Rot[2];
	const FLT x9 = x8 + x6;
	const FLT x10 = x0 * x0;
	const FLT x11 = 1 + (-2 * ((x2 * x2) + x10));
	const FLT x12 = 2 * (1. / x11);
	const FLT x13 = x0 * (*_x1).Object.Pose.Rot[2];
	const FLT x14 = -4 * x13;
	const FLT x15 = x2 * (*_x1).Object.Pose.Rot[1];
	const FLT x16 = x11 * x11;
	const FLT x17 = (x2 * x5) + (x0 * x7);
	const FLT x18 = 2 * x17 * (1. / x16);
	const FLT x19 = x16 * (1. / (x16 + (4 * (x17 * x17))));
	const FLT x20 = x7 * (*_x1).Object.Pose.Rot[3];
	const FLT x21 = x20 + (-1 * x5 * (*_x1).Object.Pose.Rot[0]);
	const FLT x22 = -4 * x1;
	const FLT x23 = x5 * (*_x1).Object.Pose.Rot[3];
	const FLT x24 = x7 * (*_x1).Object.Pose.Rot[0];
	const FLT x25 = x2 * (*_x1).Object.Pose.Rot[2];
	const FLT x26 = x0 * (*_x1).Object.Pose.Rot[1];
	const FLT x27 = x26 + x25;
	const FLT x28 = x0 * (*_x1).Object.Pose.Rot[0];
	const FLT x29 = 4 * x28;
	const FLT x30 = x2 * (*_x1).Object.Pose.Rot[3];
	const FLT x31 = x5 * (*_x1).Object.Pose.Rot[2];
	const FLT x32 = x7 * (*_x1).Object.Pose.Rot[1];
	const FLT x33 = (-1 * x32) + x31;
	const FLT x34 = (-1 * x28) + x30;
	const FLT x35 = 4 * x26;
	const FLT x36 = 2 * (1. / sqrt(1 + (-4 * (((x0 * x5) + (-1 * x2 * x7)) * ((x0 * x5) + (-1 * x2 * x7))))));
	const FLT x37 = x23 + x27 + x24;
	const FLT x38 = x21 + x13 + (-1 * x15);
	const FLT x39 = 1 + (-2 * (x10 + (x7 * x7)));
	const FLT x40 = 2 * (1. / x39);
	const FLT x41 = x39 * x39;
	const FLT x42 = (x5 * x7) + (x0 * x2);
	const FLT x43 = 2 * (1. / x41) * x42;
	const FLT x44 = x41 * (1. / (x41 + (4 * (x42 * x42))));
	const FLT x45 = (-1 * (*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[1]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[3]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[0]);
	const FLT x46 = x45 * (*_x1).Object.IMUBias.IMUCorrection[2];
	const FLT x47 = ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[2]) + ((*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[0]) + ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[3]);
	const FLT x48 = x47 * (*_x1).Object.IMUBias.IMUCorrection[1];
	const FLT x49 = x48 + x46;
	const FLT x50 = ((*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[2]);
	const FLT x51 = x50 * (*_x1).Object.IMUBias.IMUCorrection[3];
	const FLT x52 = ((*_x1).Object.IMUBias.IMUCorrection[2] * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[0] * (*_x0).Object.IMUBias.IMUCorrection[1]) + ((*_x1).Object.IMUBias.IMUCorrection[1] * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * (*_x1).Object.IMUBias.IMUCorrection[3] * (*_x0).Object.IMUBias.IMUCorrection[2]);
	const FLT x53 = x52 * (*_x1).Object.IMUBias.IMUCorrection[0];
	const FLT x54 = x53 + x51;
	const FLT x55 = x50 * x50;
	const FLT x56 = 1 + (-2 * ((x52 * x52) + x55));
	const FLT x57 = 2 * (1. / x56);
	const FLT x58 = x50 * (*_x1).Object.IMUBias.IMUCorrection[2];
	const FLT x59 = -4 * x58;
	const FLT x60 = x52 * (*_x1).Object.IMUBias.IMUCorrection[1];
	const FLT x61 = x56 * x56;
	const FLT x62 = (x52 * x47) + (x50 * x45);
	const FLT x63 = 2 * (1. / x61) * x62;
	const FLT x64 = x61 * (1. / (x61 + (4 * (x62 * x62))));
	const FLT x65 = x45 * (*_x1).Object.IMUBias.IMUCorrection[3];
	const FLT x66 = x65 + (-1 * x47 * (*_x1).Object.IMUBias.IMUCorrection[0]);
	const FLT x67 = -4 * x51;
	const FLT x68 = x47 * (*_x1).Object.IMUBias.IMUCorrection[3];
	const FLT x69 = x45 * (*_x1).Object.IMUBias.IMUCorrection[0];
	const FLT x70 = x52 * (*_x1).Object.IMUBias.IMUCorrection[2];
	const FLT x71 = x50 * (*_x1).Object.IMUBias.IMUCorrection[1];
	const FLT x72 = x71 + x70;
	const FLT x73 = x50 * (*_x1).Object.IMUBias.IMUCorrection[0];
	const FLT x74 = 4 * x73;
	const FLT x75 = x52 * (*_x1).Object.IMUBias.IMUCorrection[3];
	const FLT x76 = x75 + (-1 * x73);
	const FLT x77 = x47 * (*_x1).Object.IMUBias.IMUCorrection[2];
	const FLT x78 = x45 * (*_x1).Object.IMUBias.IMUCorrection[1];
	const FLT x79 = (-1 * x78) + x77;
	const FLT x80 = 4 * x71;
	const FLT x81 = 2 * (1. / sqrt(1 + (-4 * (((x50 * x47) + (-1 * x52 * x45)) * ((x50 * x47) + (-1 * x52 * x45))))));
	const FLT x82 = x72 + x68 + x69;
	const FLT x83 = x66 + x58 + (-1 * x60);
	const FLT x84 = 1 + (-2 * (x55 + (x45 * x45)));
	const FLT x85 = 2 * (1. / x84);
	const FLT x86 = x84 * x84;
	const FLT x87 = (x45 * x47) + (x50 * x52);
	const FLT x88 = 2 * (1. / x86) * x87;
	const FLT x89 = x86 * (1. / (x86 + (4 * (x87 * x87))));
	const FLT x90 = ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[1]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[3]);
	const FLT x91 = x90 * (*_x1).Lighthouse.Rot[3];
	const FLT x92 = (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[3]) + (-1 * (*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[0]);
	const FLT x93 = x92 * (*_x1).Lighthouse.Rot[0];
	const FLT x94 = x93 + x91;
	const FLT x95 = (-1 * (*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[0]) + (-1 * (*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[2]);
	const FLT x96 = x95 * (*_x1).Lighthouse.Rot[2];
	const FLT x97 = ((*_x1).Lighthouse.Rot[1] * (*_x0).Lighthouse.Rot[1]) + ((*_x1).Lighthouse.Rot[2] * (*_x0).Lighthouse.Rot[2]) + ((*_x1).Lighthouse.Rot[3] * (*_x0).Lighthouse.Rot[3]) + ((*_x1).Lighthouse.Rot[0] * (*_x0).Lighthouse.Rot[0]);
	const FLT x98 = x97 * (*_x1).Lighthouse.Rot[1];
	const FLT x99 = x98 + x96;
	const FLT x100 = x90 * x90;
	const FLT x101 = 1 + (-2 * ((x92 * x92) + x100));
	const FLT x102 = 2 * (1. / x101);
	const FLT x103 = x90 * (*_x1).Lighthouse.Rot[2];
	const FLT x104 = -4 * x103;
	const FLT x105 = x92 * (*_x1).Lighthouse.Rot[1];
	const FLT x106 = x101 * x101;
	const FLT x107 = (x92 * x97) + (x90 * x95);
	const FLT x108 = 2 * x107 * (1. / x106);
	const FLT x109 = x106 * (1. / (x106 + (4 * (x107 * x107))));
	const FLT x110 = x95 * (*_x1).Lighthouse.Rot[3];
	const FLT x111 = x110 + (-1 * x97 * (*_x1).Lighthouse.Rot[0]);
	const FLT x112 = -4 * x91;
	const FLT x113 = x97 * (*_x1).Lighthouse.Rot[3];
	const FLT x114 = x95 * (*_x1).Lighthouse.Rot[0];
	const FLT x115 = x92 * (*_x1).Lighthouse.Rot[2];
	const FLT x116 = x90 * (*_x1).Lighthouse.Rot[1];
	const FLT x117 = x116 + x115;
	const FLT x118 = x90 * (*_x1).Lighthouse.Rot[0];
	const FLT x119 = 4 * x118;
	const FLT x120 = x92 * (*_x1).Lighthouse.Rot[3];
	const FLT x121 = x97 * (*_x1).Lighthouse.Rot[2];
	const FLT x122 = x95 * (*_x1).Lighthouse.Rot[1];
	const FLT x123 = (-1 * x122) + x121;
	const FLT x124 = (-1 * x118) + x120;
	const FLT x125 = 4 * x116;
	const FLT x126 = 2 * (1. / sqrt(1 + (-4 * (((x90 * x97) + (-1 * x92 * x95)) * ((x90 * x97) + (-1 * x92 * x95))))));
	const FLT x127 = x117 + x113 + x114;
	const FLT x128 = x111 + x103 + (-1 * x105);
	const FLT x129 = 1 + (-2 * (x100 + (x95 * x95)));
	const FLT x130 = 2 * (1. / x129);
	const FLT x131 = x129 * x129;
	const FLT x132 = (x97 * x95) + (x92 * x90);
	const FLT x133 = 2 * (1. / x131) * x132;
	const FLT x134 = x131 * (1. / (x131 + (4 * (x132 * x132))));
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), ((-1 * ((-4 * x15) + x14) * x18) + ((x9 + x4) * x12)) * x19);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x19 * ((-1 * x18 * ((4 * x3) + x22)) + (x12 * (x21 + x15 + (-1 * x13)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x19 * ((-1 * ((4 * x30) + x29) * x18) + (x12 * ((-1 * x23) + x27 + (-1 * x24)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), ((-1 * ((-4 * x25) + x35) * x18) + ((x34 + x33) * x12)) * x19);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x36 * (x28 + x33 + (-1 * x30)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x36 * x37);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x36 * x38);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), (x4 + (-1 * x6) + (-1 * x8)) * x36);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x44 * ((-1 * (x14 + (-4 * x20)) * x43) + (x40 * x37)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x44 * ((-1 * x43 * (x22 + (4 * x8))) + (x40 * (x34 + x32 + (-1 * x31)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x44 * ((-1 * (x29 + (-4 * x32)) * x43) + ((x9 + (-1 * x3) + (-1 * x1)) * x40)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x44 * ((-1 * (x35 + (4 * x24)) * x43) + (x40 * x38)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[0])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[1])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[2])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x64 * ((-1 * ((-4 * x60) + x59) * x63) + ((x54 + x49) * x57)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x64 * ((-1 * ((4 * x53) + x67) * x63) + (x57 * (x66 + x60 + (-1 * x58)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x64 * ((-1 * ((4 * x75) + x74) * x63) + (x57 * (x72 + (-1 * x68) + (-1 * x69)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x64 * ((-1 * ((-4 * x70) + x80) * x63) + ((x79 + x76) * x57)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x81 * (x79 + x73 + (-1 * x75)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x81 * x82);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x81 * x83);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x81 * (x54 + (-1 * x48) + (-1 * x46)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), x89 * ((-1 * (x59 + (-4 * x65)) * x88) + (x82 * x85)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), x89 * ((-1 * (x67 + (4 * x46)) * x88) + (x85 * (x76 + x78 + (-1 * x77)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), x89 * ((-1 * (x74 + (-4 * x78)) * x88) + (x85 * (x49 + (-1 * x51) + (-1 * x53)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), x89 * ((-1 * (x80 + (4 * x69)) * x88) + (x83 * x85)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[0])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[1])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[2])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), ((-1 * ((-4 * x105) + x104) * x108) + ((x99 + x94) * x102)) * x109);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x109 * ((-1 * x108 * ((4 * x93) + x112)) + (x102 * (x111 + (-1 * x103) + x105))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x109 * ((-1 * ((4 * x120) + x119) * x108) + (x102 * (x117 + (-1 * x113) + (-1 * x114)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), ((-1 * ((-4 * x115) + x125) * x108) + ((x124 + x123) * x102)) * x109);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x126 * (x123 + (-1 * x120) + x118));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x127 * x126);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x128 * x126);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (x94 + (-1 * x98) + (-1 * x96)) * x126);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x134 * ((-1 * (x104 + (-4 * x110)) * x133) + (x127 * x130)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x134 * ((-1 * x133 * (x112 + (4 * x96))) + (x130 * (x124 + x122 + (-1 * x121)))));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x134 * ((-1 * (x119 + (-4 * x122)) * x133) + ((x99 + (-1 * x91) + (-1 * x93)) * x130)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x134 * ((-1 * (x125 + (4 * x114)) * x133) + (x128 * x130)));
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), -1);
}

// Full version Jacobian of SurviveJointKalmanModelToErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanModelAddErrorModel(SurviveJointKalmanModel* out, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_state) {
	const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x2 = 0.5 * (*error_state).Object.Pose.AxisAngleRot[2];
	const FLT x3 = (-1 * x2 * (*_x0).Object.Pose.Rot[1]) + (x1 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2] + (x0 * (*error_state).Object.Pose.AxisAngleRot[0]);
	const FLT x4 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x5 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x6 = (-1 * x5 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_state).Object.Pose.AxisAngleRot[2]) + (-1 * x4 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[0];
	const FLT x7 = (x5 * (*error_state).Object.Pose.AxisAngleRot[1]) + (x2 * (*_x0).Object.Pose.Rot[0]) + (*_x0).Object.Pose.Rot[3] + (-1 * x4 * (*error_state).Object.Pose.AxisAngleRot[0]);
	const FLT x8 = (x1 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[1] + (x4 * (*error_state).Object.Pose.AxisAngleRot[2]);
	const FLT x9 = 1. / sqrt((x7 * x7) + (x3 * x3) + (x8 * x8) + (x6 * x6));
	const FLT x10 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[0];
	const FLT x11 = 0.5 * (*error_state).Object.IMUBias.IMUCorrection[0];
	const FLT x12 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[1];
	const FLT x13 = (x12 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (-1 * x11 * (*_x0).Object.IMUBias.IMUCorrection[2]) + (*_x0).Object.IMUBias.IMUCorrection[3] + (x10 * (*error_state).Object.IMUBias.IMUCorrection[2]);
	const FLT x14 = (-1 * x12 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (x11 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (*_x0).Object.IMUBias.IMUCorrection[2] + (x10 * (*error_state).Object.IMUBias.IMUCorrection[1]);
	const FLT x15 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[3];
	const FLT x16 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[2];
	const FLT x17 = (-1 * x11 * (*_x0).Object.IMUBias.IMUCorrection[1]) + (*_x0).Object.IMUBias.IMUCorrection[0] + (-1 * x15 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (-1 * x16 * (*error_state).Object.IMUBias.IMUCorrection[1]);
	const FLT x18 = (*_x0).Object.IMUBias.IMUCorrection[1] + (x11 * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * x15 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (x16 * (*error_state).Object.IMUBias.IMUCorrection[2]);
	const FLT x19 = 1. / sqrt((x18 * x18) + (x17 * x17) + (x13 * x13) + (x14 * x14));
	const FLT x20 = 0.5 * (*_x0).Lighthouse.Rot[3];
	const FLT x21 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[1];
	const FLT x22 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[0];
	const FLT x23 = (-1 * x20 * (*error_state).Lighthouse.AxisAngleRot[2]) + (-1 * x22 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x21 * (*_x0).Lighthouse.Rot[2]);
	const FLT x24 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[2];
	const FLT x25 = (*_x0).Lighthouse.Rot[3] + (-1 * x22 * (*_x0).Lighthouse.Rot[2]) + (x21 * (*_x0).Lighthouse.Rot[1]) + (x24 * (*_x0).Lighthouse.Rot[0]);
	const FLT x26 = (x21 * (*_x0).Lighthouse.Rot[0]) + (-1 * x24 * (*_x0).Lighthouse.Rot[1]) + (x20 * (*error_state).Lighthouse.AxisAngleRot[0]) + (*_x0).Lighthouse.Rot[2];
	const FLT x27 = (*_x0).Lighthouse.Rot[1] + (x22 * (*_x0).Lighthouse.Rot[0]) + (-1 * x20 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x24 * (*_x0).Lighthouse.Rot[2]);
	const FLT x28 = 1. / sqrt((x27 * x27) + (x26 * x26) + (x23 * x23) + (x25 * x25));
	out->Object.Pose.Pos[0]=(*_x0).Object.Pose.Pos[0] + (*error_state).Object.Pose.Pos[0];
	out->Object.Pose.Pos[1]=(*_x0).Object.Pose.Pos[1] + (*error_state).Object.Pose.Pos[1];
	out->Object.Pose.Pos[2]=(*_x0).Object.Pose.Pos[2] + (*error_state).Object.Pose.Pos[2];
	out->Object.Pose.Rot[0]=x6 * x9;
	out->Object.Pose.Rot[1]=x8 * x9;
	out->Object.Pose.Rot[2]=x3 * x9;
	out->Object.Pose.Rot[3]=x7 * x9;
	out->Object.Velocity.Pos[0]=(*_x0).Object.Velocity.Pos[0] + (*error_state).Object.Velocity.Pos[0];
	out->Object.Velocity.Pos[1]=(*_x0).Object.Velocity.Pos[1] + (*error_state).Object.Velocity.Pos[1];
	out->Object.Velocity.Pos[2]=(*_x0).Object.Velocity.Pos[2] + (*error_state).Object.Velocity.Pos[2];
	out->Object.Velocity.AxisAngleRot[0]=(*_x0).Object.Velocity.AxisAngleRot[0] + (*error_state).Object.Velocity.AxisAngleRot[0];
	out->Object.Velocity.AxisAngleRot[1]=(*_x0).Object.Velocity.AxisAngleRot[1] + (*error_state).Object.Velocity.AxisAngleRot[1];
	out->Object.Velocity.AxisAngleRot[2]=(*_x0).Object.Velocity.AxisAngleRot[2] + (*error_state).Object.Velocity.AxisAngleRot[2];
	out->Object.Acc[0]=(*_x0).Object.Acc[0] + (*error_state).Object.Acc[0];
	out->Object.Acc[1]=(*_x0).Object.Acc[1] + (*error_state).Object.Acc[1];
	out->Object.Acc[2]=(*_x0).Object.Acc[2] + (*error_state).Object.Acc[2];
	out->Object.IMUBias.AccScale[0]=(*_x0).Object.IMUBias.AccScale[0] + (*error_state).Object.IMUBias.AccScale[0];
	out->Object.IMUBias.AccScale[1]=(*_x0).Object.IMUBias.AccScale[1] + (*error_state).Object.IMUBias.AccScale[1];
	out->Object.IMUBias.AccScale[2]=(*_x0).Object.IMUBias.AccScale[2] + (*error_state).Object.IMUBias.AccScale[2];
	out->Object.IMUBias.IMUCorrection[0]=x19 * x17;
	out->Object.IMUBias.IMUCorrection[1]=x19 * x18;
	out->Object.IMUBias.IMUCorrection[2]=x14 * x19;
	out->Object.IMUBias.IMUCorrection[3]=x13 * x19;
	out->Object.IMUBias.AccBias[0]=(*_x0).Object.IMUBias.AccBias[0] + (*error_state).Object.IMUBias.AccBias[0];
	out->Object.IMUBias.AccBias[1]=(*_x0).Object.IMUBias.AccBias[1] + (*error_state).Object.IMUBias.AccBias[1];
	out->Object.IMUBias.AccBias[2]=(*_x0).Object.IMUBias.AccBias[2] + (*error_state).Object.IMUBias.AccBias[2];
	out->Object.IMUBias.GyroBias[0]=(*_x0).Object.IMUBias.GyroBias[0] + (*error_state).Object.IMUBias.GyroBias[0];
	out->Object.IMUBias.GyroBias[1]=(*_x0).Object.IMUBias.GyroBias[1] + (*error_state).Object.IMUBias.GyroBias[1];
	out->Object.IMUBias.GyroBias[2]=(*_x0).Object.IMUBias.GyroBias[2] + (*error_state).Object.IMUBias.GyroBias[2];
	out->Lighthouse.Pos[0]=(*_x0).Lighthouse.Pos[0] + (*error_state).Lighthouse.Pos[0];
	out->Lighthouse.Pos[1]=(*_x0).Lighthouse.Pos[1] + (*error_state).Lighthouse.Pos[1];
	out->Lighthouse.Pos[2]=(*_x0).Lighthouse.Pos[2] + (*error_state).Lighthouse.Pos[2];
	out->Lighthouse.Rot[0]=x23 * x28;
	out->Lighthouse.Rot[1]=x28 * x27;
	out->Lighthouse.Rot[2]=x28 * x26;
	out->Lighthouse.Rot[3]=x25 * x28;
	out->BSD0.phase=(*error_state).BSD0.phase + (*_x0).BSD0.phase;
	out->BSD0.tilt=(*error_state).BSD0.tilt + (*_x0).BSD0.tilt;
	out->BSD0.curve=(*error_state).BSD0.curve + (*_x0).BSD0.curve;
	out->BSD0.gibpha=(*error_state).BSD0.gibpha + (*_x0).BSD0.gibpha;
	out->BSD0.gibmag=(*error_state).BSD0.gibmag + (*_x0).BSD0.gibmag;
	out->BSD0.ogeephase=(*error_state).BSD0.ogeephase + (*_x0).BSD0.ogeephase;
	out->BSD0.ogeemag=(*error_state).BSD0.ogeemag + (*_x0).BSD0.ogeemag;
	out->BSD1.phase=(*error_state).BSD1.phase + (*_x0).BSD1.phase;
	out->BSD1.tilt=(*error_state).BSD1.tilt + (*_x0).BSD1.tilt;
	out->BSD1.curve=(*error_state).BSD1.curve + (*_x0).BSD1.curve;
	out->BSD1.gibpha=(*error_state).BSD1.gibpha + (*_x0).BSD1.gibpha;
	out->BSD1.gibmag=(*error_state).BSD1.gibmag + (*_x0).BSD1.gibmag;
	out->BSD1.ogeephase=(*error_state).BSD1.ogeephase + (*_x0).BSD1.ogeephase;
	out->BSD1.ogeemag=(*error_state).BSD1.ogeemag + (*_x0).BSD1.ogeemag;
}

// Jacobian of SurviveJointKalmanModelAddErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanModelAddErrorModel_jac_x0(CnMat* Hx, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_state) {
	const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x2 = 0.5 * (*error_state).Object.Pose.AxisAngleRot[2];
	const FLT x3 = (-1 * x2 * (*_x0).Object.Pose.Rot[1]) + (x1 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2] + (x0 * (*error_state).Object.Pose.AxisAngleRot[0]);
	const FLT x4 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x5 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x6 = (-1 * x5 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x2 * (*_x0).Object.Pose.Rot[3]) + (-1 * x4 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[0];
	const FLT x7 = (x5 * (*error_state).Object.Pose.AxisAngleRot[1]) + (x2 * (*_x0).Object.Pose.Rot[0]) + (*_x0).Object.Pose.Rot[3] + (-1 * x4 * (*error_state).Object.Pose.AxisAngleRot[0]);
	const FLT x8 = (*_x0).Object.Pose.Rot[1] + (x1 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_state).Object.Pose.AxisAngleRot[1]) + (x2 * (*_x0).Object.Pose.Rot[2]);
	const FLT x9 = (x7 * x7) + (x3 * x3) + (x8 * x8) + (x6 * x6);
	const FLT x10 = 1. / sqrt(x9);
	const FLT x11 = 1.0 * x8;
	const FLT x12 = 1.0 * x3;
	const FLT x13 = 1.0 * x7;
	const FLT x14 = (x13 * (*error_state).Object.Pose.AxisAngleRot[2]) + (2 * x6) + (x11 * (*error_state).Object.Pose.AxisAngleRot[0]) + (x12 * (*error_state).Object.Pose.AxisAngleRot[1]);
	const FLT x15 = 1.0/2.0 * (1. / (x9 * sqrt(x9)));
	const FLT x16 = x15 * x14;
	const FLT x17 = 0.5 * x10;
	const FLT x18 = x17 * (*error_state).Object.Pose.AxisAngleRot[0];
	const FLT x19 = -1 * x18;
	const FLT x20 = 1.0 * x6;
	const FLT x21 = (2 * x8) + (x13 * (*error_state).Object.Pose.AxisAngleRot[1]) + (-1 * x20 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x12 * (*error_state).Object.Pose.AxisAngleRot[2]);
	const FLT x22 = x21 * x15;
	const FLT x23 = x17 * (*error_state).Object.Pose.AxisAngleRot[1];
	const FLT x24 = -1 * x23;
	const FLT x25 = x15 * ((-1 * x20 * (*error_state).Object.Pose.AxisAngleRot[1]) + (2 * x3) + (x11 * (*error_state).Object.Pose.AxisAngleRot[2]) + (-1 * x13 * (*error_state).Object.Pose.AxisAngleRot[0]));
	const FLT x26 = x17 * (*error_state).Object.Pose.AxisAngleRot[2];
	const FLT x27 = -1 * x26;
	const FLT x28 = (-1 * x20 * (*error_state).Object.Pose.AxisAngleRot[2]) + (-1 * x11 * (*error_state).Object.Pose.AxisAngleRot[1]) + (2 * x7) + (x12 * (*error_state).Object.Pose.AxisAngleRot[0]);
	const FLT x29 = x28 * x15;
	const FLT x30 = x3 * x15;
	const FLT x31 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[0];
	const FLT x32 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[2];
	const FLT x33 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[1];
	const FLT x34 = (x33 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (-1 * x32 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (*_x0).Object.IMUBias.IMUCorrection[3] + (x31 * (*error_state).Object.IMUBias.IMUCorrection[2]);
	const FLT x35 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[3];
	const FLT x36 = (-1 * x33 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (*_x0).Object.IMUBias.IMUCorrection[2] + (x35 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (x31 * (*error_state).Object.IMUBias.IMUCorrection[1]);
	const FLT x37 = (-1 * x33 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (*_x0).Object.IMUBias.IMUCorrection[0] + (-1 * x35 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (-1 * x32 * (*error_state).Object.IMUBias.IMUCorrection[1]);
	const FLT x38 = (x31 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (-1 * x35 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (*_x0).Object.IMUBias.IMUCorrection[1] + (x32 * (*error_state).Object.IMUBias.IMUCorrection[2]);
	const FLT x39 = (x38 * x38) + (x37 * x37) + (x34 * x34) + (x36 * x36);
	const FLT x40 = 1. / sqrt(x39);
	const FLT x41 = 1.0 * x38;
	const FLT x42 = 1.0 * x34;
	const FLT x43 = 1.0 * x36;
	const FLT x44 = 1.0/2.0 * (1. / (x39 * sqrt(x39)));
	const FLT x45 = x44 * ((x43 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (x42 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (2 * x37) + (x41 * (*error_state).Object.IMUBias.IMUCorrection[0]));
	const FLT x46 = 0.5 * x40;
	const FLT x47 = x46 * (*error_state).Object.IMUBias.IMUCorrection[0];
	const FLT x48 = -1 * x47;
	const FLT x49 = 1.0 * x37;
	const FLT x50 = x44 * ((-1 * x43 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (-1 * x49 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (2 * x38) + (x42 * (*error_state).Object.IMUBias.IMUCorrection[1]));
	const FLT x51 = x46 * (*error_state).Object.IMUBias.IMUCorrection[1];
	const FLT x52 = -1 * x51;
	const FLT x53 = x44 * ((x41 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (2 * x36) + (-1 * x49 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (-1 * x42 * (*error_state).Object.IMUBias.IMUCorrection[0]));
	const FLT x54 = x46 * (*error_state).Object.IMUBias.IMUCorrection[2];
	const FLT x55 = -1 * x54;
	const FLT x56 = x44 * ((x43 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (-1 * x49 * (*error_state).Object.IMUBias.IMUCorrection[2]) + (-1 * x41 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (2 * x34));
	const FLT x57 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[2];
	const FLT x58 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[1];
	const FLT x59 = 0.5 * (*error_state).Lighthouse.AxisAngleRot[0];
	const FLT x60 = (-1 * x59 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x57 * (*_x0).Lighthouse.Rot[3]) + (-1 * x58 * (*_x0).Lighthouse.Rot[2]);
	const FLT x61 = (*_x0).Lighthouse.Rot[3] + (-1 * x59 * (*_x0).Lighthouse.Rot[2]) + (x58 * (*_x0).Lighthouse.Rot[1]) + (x57 * (*_x0).Lighthouse.Rot[0]);
	const FLT x62 = (-1 * x57 * (*_x0).Lighthouse.Rot[1]) + (x58 * (*_x0).Lighthouse.Rot[0]) + (x59 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2];
	const FLT x63 = (x59 * (*_x0).Lighthouse.Rot[0]) + (-1 * x58 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[1] + (x57 * (*_x0).Lighthouse.Rot[2]);
	const FLT x64 = (x63 * x63) + (x62 * x62) + (x60 * x60) + (x61 * x61);
	const FLT x65 = 1. / sqrt(x64);
	const FLT x66 = 1.0 * x62;
	const FLT x67 = 1.0 * x63;
	const FLT x68 = 1.0 * (*error_state).Lighthouse.AxisAngleRot[2];
	const FLT x69 = (x61 * x68) + (x66 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x67 * (*error_state).Lighthouse.AxisAngleRot[0]) + (2 * x60);
	const FLT x70 = 1.0/2.0 * (1. / (x64 * sqrt(x64)));
	const FLT x71 = x70 * x60;
	const FLT x72 = 0.5 * x65;
	const FLT x73 = x72 * (*error_state).Lighthouse.AxisAngleRot[0];
	const FLT x74 = -1 * x73;
	const FLT x75 = 1.0 * x60;
	const FLT x76 = 1.0 * x61;
	const FLT x77 = (x76 * (*error_state).Lighthouse.AxisAngleRot[1]) + (-1 * x75 * (*error_state).Lighthouse.AxisAngleRot[0]) + (2 * x63) + (-1 * x62 * x68);
	const FLT x78 = x72 * (*error_state).Lighthouse.AxisAngleRot[1];
	const FLT x79 = -1 * x78;
	const FLT x80 = (-1 * x76 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x75 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x63 * x68) + (2 * x62);
	const FLT x81 = x72 * (*error_state).Lighthouse.AxisAngleRot[2];
	const FLT x82 = -1 * x81;
	const FLT x83 = (-1 * x67 * (*error_state).Lighthouse.AxisAngleRot[1]) + (2 * x61) + (x66 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x60 * x68);
	const FLT x84 = x70 * x69;
	const FLT x85 = x70 * x77;
	const FLT x86 = x80 * x70;
	const FLT x87 = x83 * x70;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), (-1 * x6 * x16) + x10);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), (-1 * x6 * x22) + x19);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), (-1 * x6 * x25) + x24);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), (-1 * x6 * x29) + x27);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), (-1 * x8 * x16) + x18);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), (-1 * x8 * x22) + x10);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), (-1 * x8 * x25) + x26);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), (-1 * x8 * x29) + x24);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), (-1 * x30 * x14) + x23);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), (-1 * x30 * x21) + x27);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), (-1 * x3 * x25) + x10);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), (-1 * x30 * x28) + x18);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), (-1 * x7 * x16) + x26);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), (-1 * x7 * x22) + x23);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), (-1 * x7 * x25) + x19);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), (-1 * x7 * x29) + x10);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x45 * x37) + x40);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x37) + x48);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x53 * x37) + x52);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x56 * x37) + x55);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x45 * x38) + x47);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x38) + x40);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x53 * x38) + x54);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x56 * x38) + x52);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x45 * x36) + x51);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x36) + x55);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x53 * x36) + x40);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x56 * x36) + x47);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x45 * x34) + x54);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x50 * x34) + x51);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x53 * x34) + x48);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), (-1 * x56 * x34) + x40);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x71 * x69) + x65);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x71 * x77) + x74);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x80 * x71) + x79);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x83 * x71) + x82);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x84 * x63) + x73);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x85 * x63) + x65);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x86 * x63) + x81);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x87 * x63) + x79);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x84 * x62) + x78);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x85 * x62) + x82);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x86 * x62) + x65);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x87 * x62) + x73);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), (-1 * x84 * x61) + x81);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), (-1 * x85 * x61) + x78);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (-1 * x86 * x61) + x74);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), (-1 * x87 * x61) + x65);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), 1);
}

// Full version Jacobian of SurviveJointKalmanModelAddErrorModel wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
// Jacobian of SurviveJointKalmanModelAddErrorModel wrt [(*error_state).Lighthouse.AxisAngleRot[0], (*error_state).Lighthouse.AxisAngleRot[1], (*error_state).Lighthouse.AxisAngleRot[2], (*error_state).Lighthouse.Pos[0], (*error_state).Lighthouse.Pos[1], (*error_state).Lighthouse.Pos[2], (*error_state).Object.Acc[0], (*error_state).Object.Acc[1], (*error_state).Object.Acc[2], (*error_state).Object.IMUBias.AccBias[0], (*error_state).Object.IMUBias.AccBias[1], (*error_state).Object.IMUBias.AccBias[2], (*error_state).Object.IMUBias.AccScale[0], (*error_state).Object.IMUBias.AccScale[1], (*error_state).Object.IMUBias.AccScale[2], (*error_state).Object.IMUBias.GyroBias[0], (*error_state).Object.IMUBias.GyroBias[1], (*error_state).Object.IMUBias.GyroBias[2], (*error_state).Object.IMUBias.IMUCorrection[0], (*error_state).Object.IMUBias.IMUCorrection[1], (*error_state).Object.IMUBias.IMUCorrection[2], (*error_state).Object.Pose.AxisAngleRot[0], (*error_state).Object.Pose.AxisAngleRot[1], (*error_state).Object.Pose.AxisAngleRot[2], (*error_state).Object.Pose.Pos[0], (*error_state).Object.Pose.Pos[1], (*error_state).Object.Pose.Pos[2], (*error_state).Object.Velocity.AxisAngleRot[0], (*error_state).Object.Velocity.AxisAngleRot[1], (*error_state).Object.Velocity.AxisAngleRot[2], (*error_state).Object.Velocity.Pos[0], (*error_state).Object.Velocity.Pos[1], (*error_state).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanModelAddErrorModel_jac_error_state(CnMat* Hx, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_state) {
	const FLT x0 = 0.5 * (*error_state).Object.Pose.AxisAngleRot[0];
	const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x2 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x3 = (-1 * x2 * (*error_state).Object.Pose.AxisAngleRot[2]) + (x1 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2] + (x0 * (*_x0).Object.Pose.Rot[3]);
	const FLT x4 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x5 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x6 = (-1 * x5 * (*error_state).Object.Pose.AxisAngleRot[2]) + (-1 * x4 * (*error_state).Object.Pose.AxisAngleRot[1]) + (-1 * x2 * (*error_state).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0];
	const FLT x7 = (x2 * (*error_state).Object.Pose.AxisAngleRot[1]) + (x1 * (*error_state).Object.Pose.AxisAngleRot[2]) + (*_x0).Object.Pose.Rot[3] + (-1 * x0 * (*_x0).Object.Pose.Rot[2]);
	const FLT x8 = (x1 * (*error_state).Object.Pose.AxisAngleRot[0]) + (-1 * x5 * (*error_state).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[1] + (x4 * (*error_state).Object.Pose.AxisAngleRot[2]);
	const FLT x9 = (x7 * x7) + (x3 * x3) + (x8 * x8) + (x6 * x6);
	const FLT x10 = 1. / sqrt(x9);
	const FLT x11 = x2 * x10;
	const FLT x12 = -1 * x11;
	const FLT x13 = 1.0 * x8;
	const FLT x14 = 1.0 * x7;
	const FLT x15 = 1.0 * x3;
	const FLT x16 = 1.0 * (*_x0).Object.Pose.Rot[1];
	const FLT x17 = 1.0/2.0 * (1. / (x9 * sqrt(x9)));
	const FLT x18 = x17 * ((x15 * (*_x0).Object.Pose.Rot[3]) + (-1 * x6 * x16) + (x13 * (*_x0).Object.Pose.Rot[0]) + (-1 * x14 * (*_x0).Object.Pose.Rot[2]));
	const FLT x19 = x4 * x10;
	const FLT x20 = -1 * x19;
	const FLT x21 = 1.0 * x6;
	const FLT x22 = x17 * ((-1 * x21 * (*_x0).Object.Pose.Rot[2]) + (x15 * (*_x0).Object.Pose.Rot[0]) + (-1 * x13 * (*_x0).Object.Pose.Rot[3]) + (x7 * x16));
	const FLT x23 = x5 * x10;
	const FLT x24 = -1 * x23;
	const FLT x25 = x17 * ((-1 * x21 * (*_x0).Object.Pose.Rot[3]) + (x13 * (*_x0).Object.Pose.Rot[2]) + (x14 * (*_x0).Object.Pose.Rot[0]) + (-1 * x3 * x16));
	const FLT x26 = x1 * x10;
	const FLT x27 = 0.5 * (*error_state).Object.IMUBias.IMUCorrection[2];
	const FLT x28 = 0.5 * (*error_state).Object.IMUBias.IMUCorrection[0];
	const FLT x29 = 0.5 * (*_x0).Object.IMUBias.IMUCorrection[1];
	const FLT x30 = (x29 * (*error_state).Object.IMUBias.IMUCorrection[1]) + (-1 * x28 * (*_x0).Object.IMUBias.IMUCorrection[2]) + (*_x0).Object.IMUBias.IMUCorrection[3] + (x27 * (*_x0).Object.IMUBias.IMUCorrection[0]);
	const FLT x31 = 0.5 * (*error_state).Object.IMUBias.IMUCorrection[1];
	const FLT x32 = (-1 * x27 * (*_x0).Object.IMUBias.IMUCorrection[1]) + (x28 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (*_x0).Object.IMUBias.IMUCorrection[2] + (x31 * (*_x0).Object.IMUBias.IMUCorrection[0]);
	const FLT x33 = (-1 * x29 * (*error_state).Object.IMUBias.IMUCorrection[0]) + (*_x0).Object.IMUBias.IMUCorrection[0] + (-1 * x27 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (-1 * x31 * (*_x0).Object.IMUBias.IMUCorrection[2]);
	const FLT x34 = (*_x0).Object.IMUBias.IMUCorrection[1] + (-1 * x31 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (x28 * (*_x0).Object.IMUBias.IMUCorrection[0]) + (x27 * (*_x0).Object.IMUBias.IMUCorrection[2]);
	const FLT x35 = (x34 * x34) + (x33 * x33) + (x30 * x30) + (x32 * x32);
	const FLT x36 = 1. / sqrt(x35);
	const FLT x37 = x36 * x29;
	const FLT x38 = -1 * x37;
	const FLT x39 = 1.0 * x33;
	const FLT x40 = 1.0 * x30;
	const FLT x41 = 1.0 * x34;
	const FLT x42 = 1.0 * x32;
	const FLT x43 = (-1 * x39 * (*_x0).Object.IMUBias.IMUCorrection[1]) + (x42 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (x41 * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * x40 * (*_x0).Object.IMUBias.IMUCorrection[2]);
	const FLT x44 = 1.0/2.0 * (1. / (x35 * sqrt(x35)));
	const FLT x45 = x44 * x33;
	const FLT x46 = 0.5 * x36;
	const FLT x47 = x46 * (*_x0).Object.IMUBias.IMUCorrection[2];
	const FLT x48 = -1 * x47;
	const FLT x49 = (x42 * (*_x0).Object.IMUBias.IMUCorrection[0]) + (-1 * x41 * (*_x0).Object.IMUBias.IMUCorrection[3]) + (x40 * (*_x0).Object.IMUBias.IMUCorrection[1]) + (-1 * x39 * (*_x0).Object.IMUBias.IMUCorrection[2]);
	const FLT x50 = x46 * (*_x0).Object.IMUBias.IMUCorrection[3];
	const FLT x51 = -1 * x50;
	const FLT x52 = (x40 * (*_x0).Object.IMUBias.IMUCorrection[0]) + (x41 * (*_x0).Object.IMUBias.IMUCorrection[2]) + (-1 * x42 * (*_x0).Object.IMUBias.IMUCorrection[1]) + (-1 * x39 * (*_x0).Object.IMUBias.IMUCorrection[3]);
	const FLT x53 = x46 * (*_x0).Object.IMUBias.IMUCorrection[0];
	const FLT x54 = x43 * x44;
	const FLT x55 = x44 * x49;
	const FLT x56 = x52 * x44;
	const FLT x57 = x44 * x30;
	const FLT x58 = 0.5 * (*_x0).Lighthouse.Rot[3];
	const FLT x59 = 0.5 * (*_x0).Lighthouse.Rot[2];
	const FLT x60 = 0.5 * (*_x0).Lighthouse.Rot[1];
	const FLT x61 = (*_x0).Lighthouse.Rot[0] + (-1 * x58 * (*error_state).Lighthouse.AxisAngleRot[2]) + (-1 * x60 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x59 * (*error_state).Lighthouse.AxisAngleRot[1]);
	const FLT x62 = 0.5 * (*_x0).Lighthouse.Rot[0];
	const FLT x63 = (x60 * (*error_state).Lighthouse.AxisAngleRot[1]) + (*_x0).Lighthouse.Rot[3] + (-1 * x59 * (*error_state).Lighthouse.AxisAngleRot[0]) + (x62 * (*error_state).Lighthouse.AxisAngleRot[2]);
	const FLT x64 = (-1 * x60 * (*error_state).Lighthouse.AxisAngleRot[2]) + (x58 * (*error_state).Lighthouse.AxisAngleRot[0]) + (x62 * (*error_state).Lighthouse.AxisAngleRot[1]) + (*_x0).Lighthouse.Rot[2];
	const FLT x65 = (*_x0).Lighthouse.Rot[1] + (x62 * (*error_state).Lighthouse.AxisAngleRot[0]) + (-1 * x58 * (*error_state).Lighthouse.AxisAngleRot[1]) + (x59 * (*error_state).Lighthouse.AxisAngleRot[2]);
	const FLT x66 = (x64 * x64) + (x65 * x65) + (x61 * x61) + (x63 * x63);
	const FLT x67 = 0.5 * (1. / sqrt(x66));
	const FLT x68 = x67 * (*_x0).Lighthouse.Rot[1];
	const FLT x69 = -1 * x68;
	const FLT x70 = 1.0 * (*_x0).Lighthouse.Rot[0];
	const FLT x71 = 1.0 * x64;
	const FLT x72 = 1.0 * x61;
	const FLT x73 = 1.0 * x63;
	const FLT x74 = (x70 * x65) + (-1 * x73 * (*_x0).Lighthouse.Rot[2]) + (-1 * x72 * (*_x0).Lighthouse.Rot[1]) + (x71 * (*_x0).Lighthouse.Rot[3]);
	const FLT x75 = 1.0/2.0 * (1. / (x66 * sqrt(x66)));
	const FLT x76 = x75 * x61;
	const FLT x77 = x67 * (*_x0).Lighthouse.Rot[2];
	const FLT x78 = -1 * x77;
	const FLT x79 = 1.0 * x65;
	const FLT x80 = (x73 * (*_x0).Lighthouse.Rot[1]) + (-1 * x79 * (*_x0).Lighthouse.Rot[3]) + (x70 * x64) + (-1 * x72 * (*_x0).Lighthouse.Rot[2]);
	const FLT x81 = x67 * (*_x0).Lighthouse.Rot[3];
	const FLT x82 = -1 * x81;
	const FLT x83 = (-1 * x72 * (*_x0).Lighthouse.Rot[3]) + (x79 * (*_x0).Lighthouse.Rot[2]) + (x70 * x63) + (-1 * x71 * (*_x0).Lighthouse.Rot[1]);
	const FLT x84 = x67 * (*_x0).Lighthouse.Rot[0];
	const FLT x85 = x75 * x74;
	const FLT x86 = x80 * x75;
	const FLT x87 = x83 * x75;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x6 * x18) + x12);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x6 * x22) + x20);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x6 * x25) + x24);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x8 * x18) + x26);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x8 * x22) + x24);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x8 * x25) + x19);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x3 * x18) + x23);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x3 * x22) + x26);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x3 * x25) + x12);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), (-1 * x7 * x18) + x20);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), (-1 * x7 * x22) + x11);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), (-1 * x7 * x25) + x26);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccScale[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccScale[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x43 * x45) + x38);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x45 * x49) + x48);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x52 * x45) + x51);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x54 * x34) + x53);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x55 * x34) + x51);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x56 * x34) + x47);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x54 * x32) + x50);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x55 * x32) + x53);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x56 * x32) + x38);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[0])/sizeof(FLT), (-1 * x57 * x43) + x48);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[1])/sizeof(FLT), (-1 * x57 * x49) + x37);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.IMUCorrection[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.IMUCorrection[2])/sizeof(FLT), (-1 * x52 * x57) + x53);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.AccBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.AccBias[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Object.IMUBias.GyroBias[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x74 * x76) + x69);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x80 * x76) + x78);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x83 * x76) + x82);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x85 * x65) + x84);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x86 * x65) + x82);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x87 * x65) + x77);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x85 * x64) + x81);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x86 * x64) + x84);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x87 * x64) + x69);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (-1 * x85 * x63) + x78);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), (-1 * x86 * x63) + x68);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (-1 * x87 * x63) + x84);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.phase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.tilt)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.curve)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), 1);
	cnMatrixOptionalSet(Hx, offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), offsetof(SurviveJointKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), 1);
}

// Full version Jacobian of SurviveJointKalmanModelAddErrorModel wrt [(*error_state).Lighthouse.AxisAngleRot[0], (*error_state).Lighthouse.AxisAngleRot[1], (*error_state).Lighthouse.AxisAngleRot[2], (*error_state).Lighthouse.Pos[0], (*error_state).Lighthouse.Pos[1], (*error_state).Lighthouse.Pos[2], (*error_state).Object.Acc[0], (*error_state).Object.Acc[1], (*error_state).Object.Acc[2], (*error_state).Object.IMUBias.AccBias[0], (*error_state).Object.IMUBias.AccBias[1], (*error_state).Object.IMUBias.AccBias[2], (*error_state).Object.IMUBias.AccScale[0], (*error_state).Object.IMUBias.AccScale[1], (*error_state).Object.IMUBias.AccScale[2], (*error_state).Object.IMUBias.GyroBias[0], (*error_state).Object.IMUBias.GyroBias[1], (*error_state).Object.IMUBias.GyroBias[2], (*error_state).Object.IMUBias.IMUCorrection[0], (*error_state).Object.IMUBias.IMUCorrection[1], (*error_state).Object.IMUBias.IMUCorrection[2], (*error_state).Object.Pose.AxisAngleRot[0], (*error_state).Object.Pose.AxisAngleRot[1], (*error_state).Object.Pose.AxisAngleRot[2], (*error_state).Object.Pose.Pos[0], (*error_state).Object.Pose.Pos[1], (*error_state).Object.Pose.Pos[2], (*error_state).Object.Velocity.AxisAngleRot[0], (*error_state).Object.Velocity.AxisAngleRot[1], (*error_state).Object.Velocity.AxisAngleRot[2], (*error_state).Object.Velocity.Pos[0], (*error_state).Object.Velocity.Pos[1], (*error_state).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline FLT SurviveJointKalmanErrorModel_LightMeas_x_gen1(const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x3 = 0.5 * (*_x0).Lighthouse.Rot[1];
	const FLT x4 = (-1 * x3 * (*error_model).Lighthouse.AxisAngleRot[0]) + (*_x0).Lighthouse.Rot[0] + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x5 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x6 = (x3 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x5 * (*_x0).Lighthouse.Rot[2]);
	const FLT x7 = x6 * x6;
	const FLT x8 = x4 * x4;
	const FLT x9 = (-1 * x3 * (*error_model).Lighthouse.AxisAngleRot[2]) + (x5 * (*_x0).Lighthouse.Rot[3]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2];
	const FLT x10 = x9 * x9;
	const FLT x11 = (*_x0).Lighthouse.Rot[1] + (x5 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (x1 * (*_x0).Lighthouse.Rot[2]);
	const FLT x12 = x11 * x11;
	const FLT x13 = x10 + x12 + x7 + x8;
	const FLT x14 = 1. / x13;
	const FLT x15 = (1. / sqrt((x14 * x12) + (x8 * x14) + (x7 * x14) + (x14 * x10))) * (1. / sqrt(x13));
	const FLT x16 = x4 * x15;
	const FLT x17 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x18 = x6 * x15;
	const FLT x19 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x20 = x15 * x11;
	const FLT x21 = (x20 * x19) + (x0 * x16) + (-1 * x18 * x17);
	const FLT x22 = x9 * x15;
	const FLT x23 = (x22 * x17) + (x19 * x16) + (-1 * x0 * x20);
	const FLT x24 = 1.0/2.0 * dt * fabs(dt);
	const FLT x25 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x26 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x27 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x28 = (-1 * x27 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x26 * (*_x0).Object.Pose.Rot[0]) + (x25 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2];
	const FLT x29 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x30 = dt * dt;
	const FLT x31 = x30 * (x29 * x29);
	const FLT x32 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x33 = x30 * (x32 * x32);
	const FLT x34 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x35 = x30 * (x34 * x34);
	const FLT x36 = 1e-10 + x35 + x31 + x33;
	const FLT x37 = sqrt(x36);
	const FLT x38 = 0.5 * x37;
	const FLT x39 = sin(x38);
	const FLT x40 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x41 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x42 = (x27 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x41 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x40 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x43 = (-1 * x27 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x25 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x26 * (*_x0).Object.Pose.Rot[2]);
	const FLT x44 = (x40 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[1] + (-1 * x26 * (*_x0).Object.Pose.Rot[3]) + (x41 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x45 = (1. / x36) * (x39 * x39);
	const FLT x46 = cos(x38);
	const FLT x47 = (1. / sqrt((x44 * x44) + (x43 * x43) + (x42 * x42) + (x28 * x28))) * (1. / sqrt((x46 * x46) + (x45 * x33) + (x45 * x31) + (x45 * x35)));
	const FLT x48 = dt * x47 * (1. / x37) * x39;
	const FLT x49 = x48 * x29;
	const FLT x50 = x48 * x34;
	const FLT x51 = x46 * x47;
	const FLT x52 = x48 * x32;
	const FLT x53 = (x52 * x43) + (x51 * x44) + (-1 * x49 * x28) + (x50 * x42);
	const FLT x54 = (x51 * x43) + (-1 * x52 * x44) + (-1 * x42 * x49) + (-1 * x50 * x28);
	const FLT x55 = (-1 * x52 * x42) + (x44 * x49) + (x50 * x43) + (x51 * x28);
	const FLT x56 = (-1 * x55 * sensor_pt[0]) + (x54 * sensor_pt[2]) + (x53 * sensor_pt[1]);
	const FLT x57 = (x51 * x42) + (x43 * x49) + (x52 * x28) + (-1 * x50 * x44);
	const FLT x58 = (-1 * x57 * sensor_pt[1]) + (x54 * sensor_pt[0]) + (x55 * sensor_pt[2]);
	const FLT x59 = (*_x0).Object.Pose.Pos[1] + (x24 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x58 * x57) + (-1 * x53 * x56)));
	const FLT x60 = x59 * x15;
	const FLT x61 = (-1 * x53 * sensor_pt[2]) + (x54 * sensor_pt[1]) + (x57 * sensor_pt[0]);
	const FLT x62 = (x24 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0] + (*_x0).Object.Pose.Pos[0] + (2 * ((x56 * x55) + (-1 * x61 * x57)));
	const FLT x63 = (*_x0).Object.Pose.Pos[2] + (2 * ((x61 * x53) + (-1 * x58 * x55))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x24 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x64 = (x63 * x20) + (x4 * x60) + (-1 * x62 * x18);
	const FLT x65 = (x62 * x22) + (x63 * x16) + (-1 * x60 * x11);
	const FLT x66 = x62 + (-1 * (x17 + (2 * ((-1 * x22 * x23) + (x21 * x18))))) + (2 * ((-1 * x65 * x22) + (x64 * x18)));
	const FLT x67 = (x0 * x18) + (-1 * x22 * x19) + (x17 * x16);
	const FLT x68 = (x59 * x18) + (x62 * x16) + (-1 * x63 * x22);
	const FLT x69 = x63 + (-1 * (x19 + (2 * ((-1 * x20 * x21) + (x67 * x22))))) + (2 * ((-1 * x64 * x20) + (x68 * x22)));
	const FLT x70 = -1 * x69;
	const FLT x71 = x59 + (2 * ((-1 * x68 * x18) + (x65 * x20))) + (-1 * (x0 + (2 * ((-1 * x67 * x18) + (x23 * x20)))));
	const FLT x72 = (-1 * ((*error_model).BSD0.phase + (*_x0).BSD0.phase)) + (-1 * atan2(x66, x70)) + (-1 * asin((1. / sqrt((x66 * x66) + (x69 * x69))) * x71 * ((*error_model).BSD0.tilt + (*_x0).BSD0.tilt)));
	return x72 + (-1 * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag) * cos(1.5707963267949 + x72 + (*error_model).BSD0.gibpha + (*_x0).BSD0.gibpha)) + ((atan2(x71, x70) * atan2(x71, x70)) * ((*error_model).BSD0.curve + (*_x0).BSD0.curve));
}

// Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_x0(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*_x0).Lighthouse.Rot[2];
	const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x3 = (x2 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x0 * (*error_model).Lighthouse.AxisAngleRot[0]);
	const FLT x4 = x3 * x3;
	const FLT x5 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x6 = (*_x0).Lighthouse.Rot[0] + (-1 * x5 * (*_x0).Lighthouse.Rot[1]) + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (-1 * x0 * (*error_model).Lighthouse.AxisAngleRot[1]);
	const FLT x7 = x6 * x6;
	const FLT x8 = (-1 * x1 * (*_x0).Lighthouse.Rot[1]) + (x5 * (*_x0).Lighthouse.Rot[3]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2];
	const FLT x9 = x8 * x8;
	const FLT x10 = (*_x0).Lighthouse.Rot[1] + (x5 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (x0 * (*error_model).Lighthouse.AxisAngleRot[2]);
	const FLT x11 = x10 * x10;
	const FLT x12 = x11 + x9 + x4 + x7;
	const FLT x13 = 1. / x12;
	const FLT x14 = x4 * x13;
	const FLT x15 = x9 * x13;
	const FLT x16 = x13 * x11;
	const FLT x17 = (x7 * x13) + x14 + x16 + x15;
	const FLT x18 = 1. / x17;
	const FLT x19 = 2 * x10;
	const FLT x20 = x13 * x19;
	const FLT x21 = x20 * x18;
	const FLT x22 = x3 * x21;
	const FLT x23 = -1 * x22;
	const FLT x24 = 2 * x6;
	const FLT x25 = x24 * x13;
	const FLT x26 = x8 * x25 * x18;
	const FLT x27 = -1 * x26;
	const FLT x28 = x27 + x23;
	const FLT x29 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x30 = 1. / sqrt(x17);
	const FLT x31 = 1. / sqrt(x12);
	const FLT x32 = x30 * x31;
	const FLT x33 = x32 * x29;
	const FLT x34 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x35 = x32 * x34;
	const FLT x36 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x37 = x32 * x36;
	const FLT x38 = (x37 * x10) + (x6 * x33) + (-1 * x3 * x35);
	const FLT x39 = x3 * x32;
	const FLT x40 = (x8 * x35) + (x6 * x37) + (-1 * x33 * x10);
	const FLT x41 = x8 * x32;
	const FLT x42 = dt * fabs(dt);
	const FLT x43 = 1.0/2.0 * x42;
	const FLT x44 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x45 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x46 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x47 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x48 = (-1 * x47 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x46 * (*_x0).Object.Pose.Rot[0]) + (x45 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2];
	const FLT x49 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x50 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x51 = (x46 * (*_x0).Object.Pose.Rot[1]) + (-1 * x50 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x49 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x52 = (-1 * x47 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x45 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x46 * (*_x0).Object.Pose.Rot[2]);
	const FLT x53 = (*_x0).Object.Pose.Rot[1] + (x49 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x46 * (*_x0).Object.Pose.Rot[3]) + (x50 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x54 = (x53 * x53) + (x52 * x52) + (x51 * x51) + (x48 * x48);
	const FLT x55 = 1. / sqrt(x54);
	const FLT x56 = dt * dt;
	const FLT x57 = x44 * x44;
	const FLT x58 = x57 * x56;
	const FLT x59 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x60 = x59 * x59;
	const FLT x61 = x60 * x56;
	const FLT x62 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x63 = x62 * x62;
	const FLT x64 = x63 * x56;
	const FLT x65 = 1e-10 + x64 + x58 + x61;
	const FLT x66 = sqrt(x65);
	const FLT x67 = 0.5 * x66;
	const FLT x68 = sin(x67);
	const FLT x69 = x68 * x68;
	const FLT x70 = 1. / x65;
	const FLT x71 = x70 * x69;
	const FLT x72 = cos(x67);
	const FLT x73 = (x71 * x61) + (x72 * x72) + (x71 * x58) + (x71 * x64);
	const FLT x74 = 1. / sqrt(x73);
	const FLT x75 = x74 * x55;
	const FLT x76 = x68 * (1. / x66);
	const FLT x77 = dt * x76;
	const FLT x78 = x75 * x77;
	const FLT x79 = x78 * x48;
	const FLT x80 = x78 * x51;
	const FLT x81 = x72 * x55;
	const FLT x82 = x81 * x74;
	const FLT x83 = x82 * x53;
	const FLT x84 = x78 * x52;
	const FLT x85 = (x84 * x59) + x83 + (-1 * x79 * x44) + (x80 * x62);
	const FLT x86 = x82 * x52;
	const FLT x87 = x75 * x53;
	const FLT x88 = x87 * x77;
	const FLT x89 = (-1 * x88 * x59) + x86 + (-1 * x80 * x44) + (-1 * x79 * x62);
	const FLT x90 = x82 * x48;
	const FLT x91 = (-1 * x80 * x59) + (x88 * x44) + (x84 * x62) + x90;
	const FLT x92 = (-1 * x91 * sensor_pt[0]) + (x89 * sensor_pt[2]) + (x85 * sensor_pt[1]);
	const FLT x93 = x82 * x51;
	const FLT x94 = (x79 * x59) + x93 + (x84 * x44) + (-1 * x88 * x62);
	const FLT x95 = (-1 * x94 * sensor_pt[1]) + (x89 * sensor_pt[0]) + (x91 * sensor_pt[2]);
	const FLT x96 = (*_x0).Object.Pose.Pos[1] + (x43 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + sensor_pt[1] + (2 * ((x95 * x94) + (-1 * x85 * x92)));
	const FLT x97 = x96 * x32;
	const FLT x98 = (-1 * x85 * sensor_pt[2]) + (x89 * sensor_pt[1]) + (x94 * sensor_pt[0]);
	const FLT x99 = (*_x0).Object.Pose.Pos[0] + (x43 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + (2 * ((x92 * x91) + (-1 * x98 * x94)));
	const FLT x100 = x99 * x32;
	const FLT x101 = (*_x0).Object.Pose.Pos[2] + (2 * ((x85 * x98) + (-1 * x91 * x95))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x43 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x102 = x32 * x101;
	const FLT x103 = (x10 * x102) + (x6 * x97) + (-1 * x3 * x100);
	const FLT x104 = (x6 * x102) + (x8 * x100) + (-1 * x97 * x10);
	const FLT x105 = x32 * x104;
	const FLT x106 = x99 + (-1 * (x34 + (2 * ((-1 * x40 * x41) + (x38 * x39))))) + (2 * ((-1 * x8 * x105) + (x39 * x103)));
	const FLT x107 = (x3 * x33) + (-1 * x8 * x37) + (x6 * x35);
	const FLT x108 = x32 * x10;
	const FLT x109 = (x6 * x100) + (x3 * x97) + (-1 * x8 * x102);
	const FLT x110 = (-1 * (x36 + (2 * ((-1 * x38 * x108) + (x41 * x107))))) + x101 + (2 * ((-1 * x103 * x108) + (x41 * x109)));
	const FLT x111 = x110 * x110;
	const FLT x112 = 1. / x111;
	const FLT x113 = x106 * x112;
	const FLT x114 = 2 * x18;
	const FLT x115 = x15 * x114;
	const FLT x116 = x14 * x114;
	const FLT x117 = -1 + x116 + x115;
	const FLT x118 = 1. / x110;
	const FLT x119 = (x106 * x106) + x111;
	const FLT x120 = 1. / x119;
	const FLT x121 = x111 * x120;
	const FLT x122 = 2 * x106;
	const FLT x123 = 2 * x110;
	const FLT x124 = (2 * ((-1 * x39 * x109) + (x10 * x105))) + x96 + (-1 * (x29 + (2 * ((-1 * x39 * x107) + (x40 * x108)))));
	const FLT x125 = (*error_model).BSD0.tilt + (*_x0).BSD0.tilt;
	const FLT x126 = 1.0/2.0 * (1. / (x119 * sqrt(x119))) * x124 * x125;
	const FLT x127 = 2 * x3;
	const FLT x128 = x13 * x127;
	const FLT x129 = x18 * x128;
	const FLT x130 = x6 * x129;
	const FLT x131 = x8 * x21;
	const FLT x132 = -1 * x131;
	const FLT x133 = x132 + x130;
	const FLT x134 = 1. / sqrt(x119);
	const FLT x135 = x125 * x134;
	const FLT x136 = x124 * x124;
	const FLT x137 = 1. / sqrt(1 + (-1 * x120 * (x125 * x125) * x136));
	const FLT x138 = (-1 * x137 * ((x133 * x135) + (-1 * x126 * ((x28 * x123) + (x117 * x122))))) + (-1 * x121 * ((-1 * x118 * x117) + (x28 * x113)));
	const FLT x139 = -1 * x110;
	const FLT x140 = 1.5707963267949 + (*_x0).BSD0.gibpha + (*error_model).BSD0.gibpha + (-1 * atan2(x106, x139)) + (-1 * ((*error_model).BSD0.phase + (*_x0).BSD0.phase)) + (-1 * asin(x124 * x135));
	const FLT x141 = sin(x140) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag);
	const FLT x142 = x112 * x124;
	const FLT x143 = atan2(x124, x139);
	const FLT x144 = 2 * (1. / (x111 + x136)) * x111 * x143 * ((*error_model).BSD0.curve + (*_x0).BSD0.curve);
	const FLT x145 = x6 * x21;
	const FLT x146 = x8 * x129;
	const FLT x147 = -1 * x146;
	const FLT x148 = x147 + x145;
	const FLT x149 = -1 * x130;
	const FLT x150 = x149 + x132;
	const FLT x151 = x16 * x114;
	const FLT x152 = -1 + x151;
	const FLT x153 = x152 + x116;
	const FLT x154 = (-1 * x137 * ((x135 * x153) + (-1 * ((x123 * x148) + (x122 * x150)) * x126))) + (-1 * ((-1 * x118 * x150) + (x113 * x148)) * x121);
	const FLT x155 = x152 + x115;
	const FLT x156 = x23 + x26;
	const FLT x157 = -1 * x145;
	const FLT x158 = x157 + x147;
	const FLT x159 = (-1 * x137 * ((x135 * x158) + (-1 * ((x123 * x155) + (x122 * x156)) * x126))) + (-1 * ((-1 * x118 * x156) + (x113 * x155)) * x121);
	const FLT x160 = 1.0 * x8;
	const FLT x161 = x160 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x162 = 1.0 * x10;
	const FLT x163 = x162 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x164 = 1.0 * x3;
	const FLT x165 = x164 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x166 = x24 + x165 + x161 + x163;
	const FLT x167 = x30 * (1. / (x12 * sqrt(x12)));
	const FLT x168 = x167 * x166;
	const FLT x169 = x10 * x103;
	const FLT x170 = x8 * x109;
	const FLT x171 = 1. / (x12 * x12);
	const FLT x172 = x166 * x171;
	const FLT x173 = x31 * (1. / (x17 * sqrt(x17)));
	const FLT x174 = x173 * (x25 + (x13 * x161) + (-1 * x9 * x172) + (-1 * x4 * x172) + (x13 * x163) + (-1 * x11 * x172) + (-1 * x7 * x172) + (x13 * x165));
	const FLT x175 = x38 * x10;
	const FLT x176 = x8 * x107;
	const FLT x177 = 1.0 * x32;
	const FLT x178 = x177 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x179 = x109 * x178;
	const FLT x180 = x107 * x178;
	const FLT x181 = 1.0/2.0 * x10;
	const FLT x182 = x101 * x174;
	const FLT x183 = x5 * x102;
	const FLT x184 = 1.0/2.0 * x3;
	const FLT x185 = x99 * x184;
	const FLT x186 = 1.0/2.0 * x6;
	const FLT x187 = x96 * x174;
	const FLT x188 = x101 * x181;
	const FLT x189 = x96 * x168;
	const FLT x190 = -1 * x1 * x100;
	const FLT x191 = x97 + x190;
	const FLT x192 = (x174 * x185) + (-1 * x186 * x189) + x183 + x191 + (-1 * x181 * x182) + (x168 * x185) + (-1 * x187 * x186) + (-1 * x168 * x188);
	const FLT x193 = x32 * x19;
	const FLT x194 = x34 * x184;
	const FLT x195 = x36 * x181;
	const FLT x196 = x168 * x186;
	const FLT x197 = x5 * x37;
	const FLT x198 = x174 * x186;
	const FLT x199 = -1 * x1 * x35;
	const FLT x200 = x197 + x199 + (x174 * x194) + (-1 * x29 * x198) + (-1 * x174 * x195) + x33 + (x168 * x194) + (-1 * x29 * x196) + (-1 * x168 * x195);
	const FLT x201 = x29 * x184;
	const FLT x202 = x1 * x33;
	const FLT x203 = 1.0/2.0 * x8;
	const FLT x204 = x36 * x203;
	const FLT x205 = -1 * x2 * x37;
	const FLT x206 = x205 + x35;
	const FLT x207 = x206 + (x204 * x168) + (-1 * x34 * x198) + x202 + (-1 * x201 * x168) + (-1 * x201 * x174) + (-1 * x34 * x196) + (x204 * x174);
	const FLT x208 = 2 * x8;
	const FLT x209 = x32 * x208;
	const FLT x210 = x1 * x97;
	const FLT x211 = x99 * x186;
	const FLT x212 = -1 * x2 * x102;
	const FLT x213 = x203 * x101;
	const FLT x214 = (x213 * x174) + x100 + x212 + (-1 * x189 * x184) + (-1 * x187 * x184) + (-1 * x211 * x168) + x210 + (x213 * x168) + (-1 * x211 * x174);
	const FLT x215 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x216 = x32 * x215;
	const FLT x217 = (x38 * x216) + (-1 * x216 * x103);
	const FLT x218 = x217 + (x214 * x209) + (-1 * x168 * x175) + (x176 * x174) + (x200 * x193) + (-1 * x180) + (-1 * x170 * x174) + x179 + (x168 * x176) + (x169 * x168) + (-1 * x168 * x170) + (-1 * x174 * x175) + (-1 * x192 * x193) + (-1 * x209 * x207) + (x169 * x174);
	const FLT x219 = x32 * x127;
	const FLT x220 = x177 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x221 = x220 * x103;
	const FLT x222 = x3 * x103;
	const FLT x223 = x3 * x38;
	const FLT x224 = x38 * x220;
	const FLT x225 = x8 * x104;
	const FLT x226 = x34 * x203;
	const FLT x227 = x29 * x181;
	const FLT x228 = x2 * x35;
	const FLT x229 = -1 * x5 * x33;
	const FLT x230 = x229 + (x227 * x174) + (-1 * x36 * x196) + (-1 * x226 * x174) + x37 + x228 + (x227 * x168) + (-1 * x226 * x168) + (-1 * x36 * x198);
	const FLT x231 = x8 * x40;
	const FLT x232 = x99 * x203;
	const FLT x233 = x101 * x186;
	const FLT x234 = x2 * x100;
	const FLT x235 = -1 * x5 * x97;
	const FLT x236 = x235 + (-1 * x233 * x168) + x102 + (-1 * x232 * x168) + (x181 * x189) + (-1 * x186 * x182) + x234 + (-1 * x232 * x174) + (x181 * x187);
	const FLT x237 = 1.0 * x105;
	const FLT x238 = (x40 * x178) + (-1 * x237 * (*error_model).Lighthouse.AxisAngleRot[1]);
	const FLT x239 = x238 + (x225 * x174) + (-1 * x222 * x168) + (-1 * x222 * x174) + (-1 * x224) + (x219 * x192) + (x223 * x168) + (-1 * x236 * x209) + (-1 * x219 * x200) + (x225 * x168) + (-1 * x231 * x168) + (-1 * x231 * x174) + x221 + (x230 * x209) + (x223 * x174);
	const FLT x240 = x40 * x10;
	const FLT x241 = x40 * x216;
	const FLT x242 = x3 * x107;
	const FLT x243 = x10 * x104;
	const FLT x244 = x3 * x109;
	const FLT x245 = x215 * x105;
	const FLT x246 = (x220 * x107) + (-1 * x220 * x109);
	const FLT x247 = x246 + (-1 * x230 * x193) + (x240 * x168) + (-1 * x219 * x214) + (x236 * x193) + (-1 * x242 * x168) + (-1 * x243 * x168) + (-1 * x241) + (-1 * x243 * x174) + (x240 * x174) + (-1 * x242 * x174) + (x244 * x174) + (x219 * x207) + x245 + (x244 * x168);
	const FLT x248 = (-1 * x137 * ((x247 * x135) + (-1 * ((x218 * x123) + (x239 * x122)) * x126))) + (-1 * ((-1 * x239 * x118) + (x218 * x113)) * x121);
	const FLT x249 = x164 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x250 = x160 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x251 = 1.0 * x6;
	const FLT x252 = x251 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x253 = (-1 * x250) + x249 + (-1 * x252) + x19;
	const FLT x254 = x253 * x167;
	const FLT x255 = x253 * x171;
	const FLT x256 = x173 * ((-1 * x9 * x255) + (-1 * x13 * x250) + x20 + (-1 * x7 * x255) + (-1 * x13 * x252) + (-1 * x4 * x255) + (-1 * x11 * x255) + (x13 * x249));
	const FLT x257 = x96 * x256;
	const FLT x258 = x254 * x186;
	const FLT x259 = (-1 * x234) + x235;
	const FLT x260 = x259 + (-1 * x96 * x258) + (-1 * x254 * x188) + x102 + (x254 * x185) + (x256 * x185) + (-1 * x256 * x188) + (-1 * x257 * x186);
	const FLT x261 = 2 * x32;
	const FLT x262 = x261 * x103;
	const FLT x263 = x38 * x261;
	const FLT x264 = x29 * x254;
	const FLT x265 = x256 * x186;
	const FLT x266 = (-1 * x228) + x229;
	const FLT x267 = x266 + (-1 * x29 * x265) + (x256 * x194) + x37 + (-1 * x254 * x195) + (x254 * x194) + (-1 * x264 * x186) + (-1 * x256 * x195);
	const FLT x268 = x2 * x33;
	const FLT x269 = x8 * x256;
	const FLT x270 = 1.0/2.0 * x269;
	const FLT x271 = x1 * x37;
	const FLT x272 = x203 * x254;
	const FLT x273 = x5 * x35;
	const FLT x274 = (-1 * x201 * x256) + (-1 * x34 * x265) + (-1 * x264 * x184) + (-1 * x273) + (-1 * x34 * x258) + (x36 * x272) + x268 + (x36 * x270) + x271;
	const FLT x275 = x2 * x97;
	const FLT x276 = x5 * x100;
	const FLT x277 = x96 * x254;
	const FLT x278 = x1 * x102;
	const FLT x279 = x278 + (x270 * x101) + (x213 * x254) + (-1 * x257 * x184) + (-1 * x211 * x256) + (-1 * x276) + (-1 * x99 * x258) + x275 + (-1 * x277 * x184);
	const FLT x280 = x246 + (x254 * x169) + (x269 * x107) + (-1 * x262) + (-1 * x254 * x170) + (x209 * x279) + (-1 * x256 * x175) + (x254 * x176) + (-1 * x254 * x175) + (-1 * x256 * x170) + x263 + (x267 * x193) + (-1 * x209 * x274) + (-1 * x260 * x193) + (x256 * x169);
	const FLT x281 = x199 + (-1 * x197);
	const FLT x282 = x281 + (-1 * x33) + (x256 * x227) + (x264 * x181) + (-1 * x36 * x265) + (-1 * x34 * x272) + (-1 * x36 * x258) + (-1 * x34 * x270);
	const FLT x283 = -1 * x183;
	const FLT x284 = x190 + (-1 * x99 * x270) + (x257 * x181) + (-1 * x99 * x272) + (x277 * x181) + (-1 * x97) + (-1 * x258 * x101) + x283 + (-1 * x233 * x256);
	const FLT x285 = (x103 * x178) + (-1 * x38 * x178);
	const FLT x286 = (x237 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x40 * x220);
	const FLT x287 = x286 + (-1 * x209 * x284) + (x256 * x223) + (x254 * x225) + (x269 * x104) + x285 + (-1 * x219 * x267) + (-1 * x40 * x269) + (-1 * x254 * x222) + (-1 * x231 * x254) + (-1 * x256 * x222) + (x209 * x282) + (x254 * x223) + (x219 * x260);
	const FLT x288 = 2 * x105;
	const FLT x289 = x40 * x261;
	const FLT x290 = (-1 * x256 * x242) + (-1 * x179) + (x284 * x193) + (x256 * x244) + (x219 * x274) + (-1 * x254 * x242) + (x254 * x244) + (x256 * x240) + (-1 * x256 * x243) + x180 + x288 + (-1 * x219 * x279) + (x254 * x240) + (-1 * x289) + (-1 * x282 * x193) + (-1 * x254 * x243);
	const FLT x291 = (-1 * x137 * ((x290 * x135) + (-1 * ((x280 * x123) + (x287 * x122)) * x126))) + (-1 * ((-1 * x287 * x118) + (x280 * x113)) * x121);
	const FLT x292 = x162 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x293 = x3 * x215;
	const FLT x294 = x251 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x295 = (-1 * x294) + (-1 * x293) + x208 + x292;
	const FLT x296 = x295 * x167;
	const FLT x297 = x11 * x171;
	const FLT x298 = x4 * x171;
	const FLT x299 = x7 * x171;
	const FLT x300 = x9 * x171;
	const FLT x301 = ((-1 * x13 * x294) + (x13 * x208) + (-1 * x295 * x300) + (-1 * x298 * x295) + (-1 * x297 * x295) + (x13 * x292) + (-1 * x299 * x295) + (-1 * x13 * x293)) * x173;
	const FLT x302 = x301 * x186;
	const FLT x303 = x296 * x186;
	const FLT x304 = x276 + (-1 * x96 * x303) + (-1 * x275) + x278 + (x301 * x185) + (-1 * x301 * x188) + (-1 * x296 * x188) + (-1 * x96 * x302) + (x296 * x185);
	const FLT x305 = x261 * x109;
	const FLT x306 = x266 + (-1 * x201 * x296) + (-1 * x34 * x302) + (x204 * x301) + (x204 * x296) + (-1 * x34 * x303) + (-1 * x201 * x301) + (-1 * x37);
	const FLT x307 = (x301 * x194) + x273 + (-1 * x268) + x271 + (-1 * x29 * x302) + (-1 * x296 * x195) + (-1 * x29 * x303) + (-1 * x301 * x195) + (x296 * x194);
	const FLT x308 = x261 * x107;
	const FLT x309 = x96 * x301;
	const FLT x310 = x96 * x296;
	const FLT x311 = (x213 * x301) + (-1 * x102) + (x213 * x296) + (-1 * x99 * x302) + (-1 * x211 * x296) + x259 + (-1 * x309 * x184) + (-1 * x310 * x184);
	const FLT x312 = (x209 * x311) + (x296 * x176) + (-1 * x296 * x175) + (-1 * x209 * x306) + (x296 * x169) + (-1 * x296 * x170) + (-1 * x301 * x175) + (x307 * x193) + x305 + (-1 * x308) + (-1 * x304 * x193) + (-1 * x221) + (x301 * x169) + (x301 * x176) + (-1 * x301 * x170) + x224;
	const FLT x313 = -1 * x202;
	const FLT x314 = (x227 * x301) + x313 + (-1 * x36 * x302) + (-1 * x226 * x301) + x206 + (-1 * x36 * x303) + (-1 * x296 * x226) + (x296 * x227);
	const FLT x315 = x212 + (-1 * x210);
	const FLT x316 = x315 + (-1 * x232 * x301) + (x310 * x181) + (-1 * x232 * x296) + (-1 * x302 * x101) + x100 + (x309 * x181) + (-1 * x233 * x296);
	const FLT x317 = x217 + (x296 * x225) + (x219 * x304) + (-1 * x288) + (-1 * x209 * x316) + (x209 * x314) + (-1 * x296 * x222) + (x223 * x301) + (x225 * x301) + (-1 * x222 * x301) + (-1 * x231 * x301) + (-1 * x231 * x296) + x289 + (x296 * x223) + (-1 * x219 * x307);
	const FLT x318 = (-1 * x216 * x107) + (x216 * x109);
	const FLT x319 = x286 + (x296 * x244) + (x219 * x306) + (x240 * x301) + (-1 * x243 * x301) + (x316 * x193) + (x244 * x301) + (x296 * x240) + (-1 * x314 * x193) + (-1 * x296 * x242) + (-1 * x296 * x243) + x318 + (-1 * x219 * x311) + (-1 * x242 * x301);
	const FLT x320 = (-1 * x137 * ((x319 * x135) + (-1 * ((x312 * x123) + (x317 * x122)) * x126))) + (-1 * ((-1 * x317 * x118) + (x312 * x113)) * x121);
	const FLT x321 = x162 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x322 = x8 * x215;
	const FLT x323 = x251 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x324 = (-1 * x323) + x127 + (-1 * x321) + x322;
	const FLT x325 = x324 * x167;
	const FLT x326 = x173 * ((-1 * x298 * x324) + (-1 * x13 * x323) + (x13 * x322) + (-1 * x297 * x324) + (-1 * x13 * x321) + (-1 * x299 * x324) + (-1 * x300 * x324) + x128);
	const FLT x327 = x325 * x186;
	const FLT x328 = x96 * x326;
	const FLT x329 = x315 + (-1 * x100) + (-1 * x326 * x188) + (x326 * x185) + (-1 * x325 * x188) + (-1 * x96 * x327) + (x325 * x185) + (-1 * x328 * x186);
	const FLT x330 = x96 * x325;
	const FLT x331 = x203 * x326;
	const FLT x332 = x191 + (x213 * x325) + (x331 * x101) + (-1 * x330 * x184) + (-1 * x99 * x327) + x283 + (-1 * x328 * x184) + (-1 * x211 * x326);
	const FLT x333 = x29 * x326;
	const FLT x334 = x36 * x326;
	const FLT x335 = x34 * x326;
	const FLT x336 = (x335 * x184) + (-1 * x35) + x313 + (-1 * x333 * x186) + x205 + (-1 * x29 * x327) + (-1 * x325 * x195) + (-1 * x334 * x181) + (x325 * x194);
	const FLT x337 = x36 * x325;
	const FLT x338 = (x203 * x337) + (-1 * x335 * x186) + (-1 * x333 * x184) + (x36 * x331) + (-1 * x34 * x327) + x281 + x33 + (-1 * x201 * x325);
	const FLT x339 = x326 * x103;
	const FLT x340 = (x10 * x339) + x318 + (x325 * x176) + x285 + (-1 * x325 * x175) + (x336 * x193) + (x325 * x169) + (x326 * x176) + (-1 * x329 * x193) + (-1 * x325 * x170) + (-1 * x209 * x338) + (x209 * x332) + (-1 * x326 * x170) + (-1 * x326 * x175);
	const FLT x341 = (-1 * x271) + (x333 * x181) + x273 + (-1 * x334 * x186) + (x227 * x325) + (-1 * x34 * x331) + x268 + (-1 * x337 * x186) + (-1 * x226 * x325);
	const FLT x342 = (-1 * x278) + x275 + (-1 * x99 * x331) + (x328 * x181) + (x330 * x181) + x276 + (-1 * x233 * x326) + (-1 * x232 * x325) + (-1 * x327 * x101);
	const FLT x343 = (x225 * x325) + (x225 * x326) + (x223 * x326) + (-1 * x263) + x262 + (x223 * x325) + (-1 * x222 * x325) + (-1 * x3 * x339) + (x219 * x329) + (-1 * x245) + (-1 * x231 * x326) + (-1 * x209 * x342) + x241 + (-1 * x219 * x336) + (-1 * x231 * x325) + (x209 * x341);
	const FLT x344 = x308 + (-1 * x305) + x238 + (x244 * x325) + (-1 * x242 * x326) + (x342 * x193) + (-1 * x243 * x326) + (x219 * x338) + (-1 * x219 * x332) + (x244 * x326) + (x240 * x325) + (x240 * x326) + (-1 * x242 * x325) + (-1 * x243 * x325) + (-1 * x341 * x193);
	const FLT x345 = (-1 * x137 * ((x344 * x135) + (-1 * ((x340 * x123) + (x343 * x122)) * x126))) + (-1 * ((-1 * x343 * x118) + (x340 * x113)) * x121);
	const FLT x346 = x42 * x18;
	const FLT x347 = x13 * x346;
	const FLT x348 = x8 * x347;
	const FLT x349 = x6 * x348;
	const FLT x350 = x3 * x10 * x347;
	const FLT x351 = x350 + x349;
	const FLT x352 = -1 * x14 * x346;
	const FLT x353 = (-1 * x15 * x346) + x43;
	const FLT x354 = x353 + x352;
	const FLT x355 = x10 * x348;
	const FLT x356 = x6 * x347;
	const FLT x357 = x3 * x356;
	const FLT x358 = (-1 * x357) + x355;
	const FLT x359 = (-1 * x137 * ((x358 * x135) + (-1 * ((x351 * x123) + (x354 * x122)) * x126))) + (-1 * ((-1 * x354 * x118) + (x351 * x113)) * x121);
	const FLT x360 = x3 * x348;
	const FLT x361 = x10 * x356;
	const FLT x362 = (-1 * x361) + x360;
	const FLT x363 = x355 + x357;
	const FLT x364 = -1 * x16 * x346;
	const FLT x365 = x364 + x43 + x352;
	const FLT x366 = (-1 * x137 * ((x365 * x135) + (-1 * ((x362 * x123) + (x363 * x122)) * x126))) + (-1 * ((-1 * x363 * x118) + (x362 * x113)) * x121);
	const FLT x367 = x353 + x364;
	const FLT x368 = (-1 * x349) + x350;
	const FLT x369 = x360 + x361;
	const FLT x370 = (-1 * x137 * ((x369 * x135) + (-1 * ((x367 * x123) + (x368 * x122)) * x126))) + (-1 * ((-1 * x368 * x118) + (x367 * x113)) * x121);
	const FLT x371 = x22 + x26;
	const FLT x372 = -1 * x116;
	const FLT x373 = -1 * x115;
	const FLT x374 = 1 + x373 + x372;
	const FLT x375 = x149 + x131;
	const FLT x376 = (-1 * x137 * ((x375 * x135) + (-1 * ((x371 * x123) + (x374 * x122)) * x126))) + (-1 * ((-1 * x374 * x118) + (x371 * x113)) * x121);
	const FLT x377 = x157 + x146;
	const FLT x378 = x131 + x130;
	const FLT x379 = 1 + (-1 * x151);
	const FLT x380 = x379 + x372;
	const FLT x381 = (-1 * x137 * ((x380 * x135) + (-1 * ((x377 * x123) + (x378 * x122)) * x126))) + (-1 * ((-1 * x378 * x118) + (x377 * x113)) * x121);
	const FLT x382 = x379 + x373;
	const FLT x383 = x27 + x22;
	const FLT x384 = x146 + x145;
	const FLT x385 = (-1 * x137 * ((x384 * x135) + (-1 * ((x382 * x123) + (x383 * x122)) * x126))) + (-1 * ((-1 * x383 * x118) + (x382 * x113)) * x121);
	const FLT x386 = 1.0/2.0 * x48;
	const FLT x387 = x77 * x386;
	const FLT x388 = 1.0 * x53;
	const FLT x389 = 1.0 * x51;
	const FLT x390 = 1.0 * x48;
	const FLT x391 = (x390 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x389 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x388 * (*error_model).Object.Pose.AxisAngleRot[0]) + (2 * x52);
	const FLT x392 = x74 * (1. / (x54 * sqrt(x54)));
	const FLT x393 = x391 * x392;
	const FLT x394 = x59 * x393;
	const FLT x395 = 1.0/2.0 * x51;
	const FLT x396 = x72 * x392;
	const FLT x397 = x396 * x391;
	const FLT x398 = 1.0/2.0 * x52;
	const FLT x399 = x77 * x398;
	const FLT x400 = x44 * x393;
	const FLT x401 = x78 * x59;
	const FLT x402 = x46 * x401;
	const FLT x403 = 1.0/2.0 * x53;
	const FLT x404 = x77 * x403;
	const FLT x405 = x62 * x393;
	const FLT x406 = 0.5 * x82;
	const FLT x407 = x406 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x408 = x78 * x44;
	const FLT x409 = x78 * x62;
	const FLT x410 = 0.5 * x409;
	const FLT x411 = -1 * x410 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x412 = x411 + x408;
	const FLT x413 = (-1 * x395 * x397) + x412 + (-1 * x400 * x399) + x407 + x402 + (-1 * x394 * x387) + (x405 * x404);
	const FLT x414 = 2 * x98;
	const FLT x415 = 0.5 * x408;
	const FLT x416 = x415 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x417 = x82 * x46;
	const FLT x418 = x77 * x395;
	const FLT x419 = x59 * x418;
	const FLT x420 = x62 * x399;
	const FLT x421 = 0.5 * x401;
	const FLT x422 = -1 * x421 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x423 = x409 + x422;
	const FLT x424 = x423 + (-1 * x397 * x386) + x417 + (x419 * x393) + (-1 * x400 * x404) + (-1 * x420 * x393) + x416;
	const FLT x425 = x421 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x426 = -1 * x425;
	const FLT x427 = x46 * x409;
	const FLT x428 = -1 * x427;
	const FLT x429 = x415 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x430 = x82 + (-1 * x429);
	const FLT x431 = (x405 * x387) + x430 + x426 + (-1 * x398 * x397) + (x404 * x394) + x428 + (x400 * x418);
	const FLT x432 = x406 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x433 = x59 * x399;
	const FLT x434 = -1 * x46 * x408;
	const FLT x435 = x410 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x436 = x435 + x434;
	const FLT x437 = (-1 * x405 * x418) + x401 + x436 + (-1 * x433 * x393) + (-1 * x403 * x397) + x432 + (x400 * x387);
	const FLT x438 = (x437 * sensor_pt[1]) + (-1 * x424 * sensor_pt[0]) + (x431 * sensor_pt[2]);
	const FLT x439 = 2 * x91;
	const FLT x440 = (x413 * sensor_pt[0]) + (-1 * x437 * sensor_pt[2]) + (x431 * sensor_pt[1]);
	const FLT x441 = 2 * x94;
	const FLT x442 = 2 * x92;
	const FLT x443 = (x424 * x442) + (-1 * x441 * x440) + (-1 * x413 * x414) + (x438 * x439);
	const FLT x444 = x6 * x32;
	const FLT x445 = 2 * x85;
	const FLT x446 = 2 * x95;
	const FLT x447 = (x431 * sensor_pt[0]) + (x424 * sensor_pt[2]) + (-1 * x413 * sensor_pt[1]);
	const FLT x448 = (x441 * x447) + (x413 * x446) + (-1 * x437 * x442) + (-1 * x438 * x445);
	const FLT x449 = x32 * x448;
	const FLT x450 = (x445 * x440) + (x414 * x437) + (-1 * x424 * x446) + (-1 * x439 * x447);
	const FLT x451 = (-1 * x41 * x450) + (x444 * x443) + (x3 * x449);
	const FLT x452 = (-1 * x39 * x443) + (x450 * x108) + (x6 * x449);
	const FLT x453 = (x451 * x209) + x450 + (-1 * x452 * x193);
	const FLT x454 = (-1 * x10 * x449) + (x41 * x443) + (x450 * x444);
	const FLT x455 = x443 + (-1 * x454 * x209) + (x452 * x219);
	const FLT x456 = x448 + (x454 * x193) + (-1 * x451 * x219);
	const FLT x457 = (-1 * x137 * ((x456 * x135) + (-1 * ((x453 * x123) + (x455 * x122)) * x126))) + (-1 * ((-1 * x455 * x118) + (x453 * x113)) * x121);
	const FLT x458 = 1.0 * x52;
	const FLT x459 = (-1 * x390 * (*error_model).Object.Pose.AxisAngleRot[2]) + (2 * x53) + (-1 * x458 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x389 * (*error_model).Object.Pose.AxisAngleRot[1]);
	const FLT x460 = x59 * x459;
	const FLT x461 = x460 * x392;
	const FLT x462 = x459 * x392;
	const FLT x463 = x62 * x462;
	const FLT x464 = x459 * x396;
	const FLT x465 = x44 * x462;
	const FLT x466 = x429 + x82;
	const FLT x467 = x466 + (x465 * x387) + x427 + (-1 * x463 * x418) + (-1 * x464 * x403) + x426 + (-1 * x461 * x399);
	const FLT x468 = -1 * x402;
	const FLT x469 = -1 * x407;
	const FLT x470 = x404 * x392;
	const FLT x471 = x459 * x470;
	const FLT x472 = (-1 * x463 * x399) + x469 + (-1 * x464 * x386) + (x462 * x419) + x412 + (-1 * x44 * x471) + x468;
	const FLT x473 = -1 * x401;
	const FLT x474 = -1 * x432;
	const FLT x475 = x436 + x474 + (x463 * x387) + (x460 * x470) + x473 + (-1 * x464 * x398) + (x465 * x418);
	const FLT x476 = (x467 * sensor_pt[1]) + (-1 * x472 * sensor_pt[0]) + (x475 * sensor_pt[2]);
	const FLT x477 = -1 * x416;
	const FLT x478 = (-1 * x409) + x422;
	const FLT x479 = x417 + x478 + (-1 * x461 * x387) + (-1 * x465 * x399) + (x62 * x471) + x477 + (-1 * x464 * x395);
	const FLT x480 = (x472 * sensor_pt[2]) + (-1 * x479 * sensor_pt[1]) + (x475 * sensor_pt[0]);
	const FLT x481 = (x480 * x441) + (x479 * x446) + (-1 * x467 * x442) + (-1 * x476 * x445);
	const FLT x482 = (x479 * sensor_pt[0]) + (x475 * sensor_pt[1]) + (-1 * x467 * sensor_pt[2]);
	const FLT x483 = (-1 * x482 * x441) + (-1 * x479 * x414) + (x472 * x442) + (x476 * x439);
	const FLT x484 = (x467 * x414) + (-1 * x472 * x446) + (x482 * x445) + (-1 * x480 * x439);
	const FLT x485 = (x39 * x481) + (-1 * x41 * x484) + (x483 * x444);
	const FLT x486 = (-1 * x39 * x483) + (x484 * x108) + (x481 * x444);
	const FLT x487 = x484 + (x485 * x209) + (-1 * x486 * x193);
	const FLT x488 = (-1 * x481 * x108) + (x41 * x483) + (x484 * x444);
	const FLT x489 = x483 + (x486 * x219) + (-1 * x488 * x209);
	const FLT x490 = x481 + (-1 * x485 * x219) + (x488 * x193);
	const FLT x491 = (-1 * x137 * ((x490 * x135) + (-1 * ((x487 * x123) + (x489 * x122)) * x126))) + (-1 * ((-1 * x489 * x118) + (x487 * x113)) * x121);
	const FLT x492 = (2 * x48) + (x388 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x458 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x389 * (*error_model).Object.Pose.AxisAngleRot[0]);
	const FLT x493 = x492 * x392;
	const FLT x494 = x492 * x396;
	const FLT x495 = x493 * x387;
	const FLT x496 = x62 * x493;
	const FLT x497 = (-1 * x408) + x411;
	const FLT x498 = x497 + (-1 * x496 * x418) + x468 + x407 + (-1 * x493 * x433) + (x44 * x495) + (-1 * x494 * x403);
	const FLT x499 = x492 * x470;
	const FLT x500 = x44 * x399;
	const FLT x501 = x434 + (-1 * x435);
	const FLT x502 = x501 + (-1 * x494 * x395) + (-1 * x59 * x495) + x401 + (x62 * x499) + x474 + (-1 * x493 * x500);
	const FLT x503 = -1 * x417;
	const FLT x504 = x503 + (x44 * x493 * x418) + x416 + (x59 * x499) + x478 + (-1 * x494 * x398) + (x496 * x387);
	const FLT x505 = x466 + x425 + (-1 * x44 * x499) + (-1 * x494 * x386) + x428 + (-1 * x493 * x420) + (x493 * x419);
	const FLT x506 = (x505 * sensor_pt[2]) + (-1 * x502 * sensor_pt[1]) + (x504 * sensor_pt[0]);
	const FLT x507 = (x498 * sensor_pt[1]) + (-1 * x505 * sensor_pt[0]) + (x504 * sensor_pt[2]);
	const FLT x508 = (x446 * x502) + (-1 * x445 * x507) + (-1 * x498 * x442) + (x441 * x506);
	const FLT x509 = (x502 * sensor_pt[0]) + (-1 * x498 * sensor_pt[2]) + (x504 * sensor_pt[1]);
	const FLT x510 = (-1 * x441 * x509) + (x442 * x505) + (-1 * x414 * x502) + (x439 * x507);
	const FLT x511 = (x445 * x509) + (-1 * x439 * x506) + (-1 * x446 * x505) + (x498 * x414);
	const FLT x512 = (-1 * x41 * x511) + (x39 * x508) + (x444 * x510);
	const FLT x513 = (-1 * x39 * x510) + (x511 * x108) + (x444 * x508);
	const FLT x514 = x511 + (x512 * x209) + (-1 * x513 * x193);
	const FLT x515 = (-1 * x508 * x108) + (x41 * x510) + (x444 * x511);
	const FLT x516 = x510 + (x513 * x219) + (-1 * x515 * x209);
	const FLT x517 = x508 + (x515 * x193) + (-1 * x512 * x219);
	const FLT x518 = (-1 * x137 * ((x517 * x135) + (-1 * ((x514 * x123) + (x516 * x122)) * x126))) + (-1 * ((-1 * x516 * x118) + (x514 * x113)) * x121);
	const FLT x519 = (x390 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x388 * (*error_model).Object.Pose.AxisAngleRot[1]) + (2 * x51) + (-1 * x458 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x520 = x519 * x392;
	const FLT x521 = x59 * x520;
	const FLT x522 = x44 * x520;
	const FLT x523 = x519 * x396;
	const FLT x524 = x418 * x520;
	const FLT x525 = (-1 * x62 * x524) + x477 + x503 + x423 + (-1 * x521 * x399) + (x522 * x387) + (-1 * x403 * x523);
	const FLT x526 = x62 * x520;
	const FLT x527 = x497 + x469 + (x526 * x387) + x402 + (x404 * x521) + (-1 * x523 * x398) + (x44 * x524);
	const FLT x528 = (-1 * x523 * x386) + (-1 * x404 * x522) + x473 + x501 + (x419 * x520) + (-1 * x420 * x520) + x432;
	const FLT x529 = (x525 * sensor_pt[1]) + (x527 * sensor_pt[2]) + (-1 * x528 * sensor_pt[0]);
	const FLT x530 = x427 + (x404 * x526) + (-1 * x523 * x395) + (-1 * x521 * x387) + x430 + x425 + (-1 * x520 * x500);
	const FLT x531 = (x528 * sensor_pt[2]) + (-1 * x530 * sensor_pt[1]) + (x527 * sensor_pt[0]);
	const FLT x532 = (x441 * x531) + (x446 * x530) + (-1 * x442 * x525) + (-1 * x445 * x529);
	const FLT x533 = (x530 * sensor_pt[0]) + (-1 * x525 * sensor_pt[2]) + (x527 * sensor_pt[1]);
	const FLT x534 = (x442 * x528) + (-1 * x441 * x533) + (-1 * x414 * x530) + (x439 * x529);
	const FLT x535 = (x445 * x533) + (-1 * x446 * x528) + (x414 * x525) + (-1 * x439 * x531);
	const FLT x536 = (-1 * x41 * x535) + (x39 * x532) + (x444 * x534);
	const FLT x537 = x32 * x535;
	const FLT x538 = (-1 * x39 * x534) + (x10 * x537) + (x444 * x532);
	const FLT x539 = x535 + (x536 * x209) + (-1 * x538 * x193);
	const FLT x540 = (-1 * x532 * x108) + (x41 * x534) + (x6 * x537);
	const FLT x541 = x534 + (x538 * x219) + (-1 * x540 * x209);
	const FLT x542 = x532 + (-1 * x536 * x219) + (x540 * x193);
	const FLT x543 = (-1 * x137 * ((x542 * x135) + (-1 * ((x539 * x123) + (x541 * x122)) * x126))) + (-1 * ((-1 * x541 * x118) + (x539 * x113)) * x121);
	const FLT x544 = dt * dt * dt;
	const FLT x545 = 0.5 * x70 * x544;
	const FLT x546 = x90 * x545;
	const FLT x547 = x44 * x546;
	const FLT x548 = x59 * x547;
	const FLT x549 = x62 * x59;
	const FLT x550 = x545 * x549;
	const FLT x551 = x93 * x550;
	const FLT x552 = x60 * x545;
	const FLT x553 = x59 * x59 * x59;
	const FLT x554 = dt * dt * dt * dt;
	const FLT x555 = 2 * (1. / (x65 * x65)) * x69;
	const FLT x556 = x554 * x555;
	const FLT x557 = x68 * (1. / (x65 * sqrt(x65)));
	const FLT x558 = 1.0 * x72;
	const FLT x559 = x558 * x557;
	const FLT x560 = x559 * x554;
	const FLT x561 = x57 * x59;
	const FLT x562 = x63 * x556;
	const FLT x563 = 2 * x71;
	const FLT x564 = x56 * x563;
	const FLT x565 = x63 * x560;
	const FLT x566 = x76 * x558;
	const FLT x567 = x56 * x566;
	const FLT x568 = (x560 * x561) + (x553 * x560) + (x59 * x564) + (-1 * x553 * x556) + (-1 * x59 * x567) + (x59 * x565) + (-1 * x556 * x561) + (-1 * x59 * x562);
	const FLT x569 = 1. / (x73 * sqrt(x73));
	const FLT x570 = x55 * x569;
	const FLT x571 = x62 * x570;
	const FLT x572 = x571 * x568;
	const FLT x573 = x44 * x570;
	const FLT x574 = x573 * x568;
	const FLT x575 = 0.5 * x76;
	const FLT x576 = x56 * x575;
	const FLT x577 = x59 * x576;
	const FLT x578 = x81 * x569;
	const FLT x579 = x578 * x568;
	const FLT x580 = x75 * x51;
	const FLT x581 = x544 * x557;
	const FLT x582 = x62 * x581;
	const FLT x583 = x59 * x582;
	const FLT x584 = x580 * x583;
	const FLT x585 = x570 * x568;
	const FLT x586 = x75 * x44;
	const FLT x587 = x48 * x586;
	const FLT x588 = x59 * x581;
	const FLT x589 = x587 * x588;
	const FLT x590 = x60 * x581;
	const FLT x591 = x75 * x590;
	const FLT x592 = (-1 * x52 * x591) + (-1 * x418 * x572) + x551 + (-1 * x548) + (-1 * x433 * x585) + (x574 * x387) + (-1 * x87 * x577) + (-1 * x403 * x579) + (-1 * x584) + (x86 * x552) + x84 + x589;
	const FLT x593 = x59 * x585;
	const FLT x594 = (x87 * x583) + (-1 * x83 * x550);
	const FLT x595 = x44 * x545;
	const FLT x596 = x59 * x595;
	const FLT x597 = x52 * x586;
	const FLT x598 = (-1 * x597 * x588) + (x86 * x596);
	const FLT x599 = x594 + (-1 * x48 * x591) + x598 + (-1 * x579 * x395) + (-1 * x577 * x580) + (x60 * x546) + (x404 * x572) + (-1 * x593 * x387) + x79 + (-1 * x574 * x399);
	const FLT x600 = x75 * x48;
	const FLT x601 = x600 * x583;
	const FLT x602 = x418 * x573;
	const FLT x603 = x546 * x549;
	const FLT x604 = x75 * x52;
	const FLT x605 = -1 * x88;
	const FLT x606 = x93 * x595;
	const FLT x607 = x44 * x580;
	const FLT x608 = (x607 * x588) + (-1 * x59 * x606);
	const FLT x609 = (-1 * x579 * x398) + x605 + (-1 * x83 * x552) + x608 + (-1 * x604 * x577) + (x404 * x593) + (x87 * x590) + (x572 * x387) + (x602 * x568) + (-1 * x603) + x601;
	const FLT x610 = -1 * x80;
	const FLT x611 = x87 * x44;
	const FLT x612 = x611 * x588;
	const FLT x613 = x83 * x596;
	const FLT x614 = (-1 * x604 * x583) + (x86 * x550);
	const FLT x615 = x613 + (x590 * x580) + (x419 * x585) + (-1 * x612) + (-1 * x579 * x386) + (-1 * x600 * x577) + x614 + (-1 * x572 * x399) + x610 + (-1 * x404 * x574) + (-1 * x93 * x552);
	const FLT x616 = (x615 * sensor_pt[2]) + (-1 * x599 * sensor_pt[1]) + (x609 * sensor_pt[0]);
	const FLT x617 = (x592 * sensor_pt[1]) + (-1 * x615 * sensor_pt[0]) + (x609 * sensor_pt[2]);
	const FLT x618 = (-1 * x445 * x617) + (x441 * x616) + (-1 * x442 * x592) + (x446 * x599);
	const FLT x619 = x32 * x618;
	const FLT x620 = (x599 * sensor_pt[0]) + (-1 * x592 * sensor_pt[2]) + (x609 * sensor_pt[1]);
	const FLT x621 = (x442 * x615) + (-1 * x441 * x620) + (-1 * x414 * x599) + (x439 * x617);
	const FLT x622 = x32 * x621;
	const FLT x623 = (x414 * x592) + (x445 * x620) + (-1 * x446 * x615) + (-1 * x439 * x616);
	const FLT x624 = (-1 * x41 * x623) + (x3 * x619) + (x6 * x622);
	const FLT x625 = (x623 * x108) + (-1 * x3 * x622) + (x444 * x618);
	const FLT x626 = x623 + (x624 * x209) + (-1 * x625 * x193);
	const FLT x627 = x32 * ((-1 * x10 * x619) + (x8 * x622) + (x444 * x623));
	const FLT x628 = x621 + (x625 * x219) + (-1 * x627 * x208);
	const FLT x629 = x618 + (x19 * x627) + (-1 * x624 * x219);
	const FLT x630 = (-1 * x137 * ((x629 * x135) + (-1 * ((x626 * x123) + (x628 * x122)) * x126))) + (-1 * ((-1 * x628 * x118) + (x626 * x113)) * x121);
	const FLT x631 = x63 * x545;
	const FLT x632 = x63 * x581;
	const FLT x633 = x62 * x556;
	const FLT x634 = x62 * x560;
	const FLT x635 = x62 * x56;
	const FLT x636 = (x62 * x62 * x62) * x554;
	const FLT x637 = (x635 * x563) + (x636 * x559) + (-1 * x636 * x555) + (-1 * x60 * x633) + (x60 * x634) + (-1 * x635 * x566) + (-1 * x57 * x633) + (x57 * x634);
	const FLT x638 = x637 * x570;
	const FLT x639 = x59 * x638;
	const FLT x640 = x637 * x573;
	const FLT x641 = x637 * x571;
	const FLT x642 = x77 * x641;
	const FLT x643 = x637 * x578;
	const FLT x644 = x635 * x575;
	const FLT x645 = x62 * x595;
	const FLT x646 = (-1 * x597 * x582) + (x86 * x645);
	const FLT x647 = (-1 * x643 * x395) + (-1 * x601) + (x403 * x642) + (-1 * x83 * x631) + (-1 * x639 * x387) + (-1 * x644 * x580) + (-1 * x640 * x399) + x605 + x646 + (x87 * x632) + x603;
	const FLT x648 = x75 * x632;
	const FLT x649 = x611 * x582;
	const FLT x650 = x83 * x645;
	const FLT x651 = x84 + (-1 * x643 * x386) + (-1 * x551) + (-1 * x600 * x644) + (-1 * x642 * x398) + (-1 * x404 * x640) + (x419 * x638) + (-1 * x649) + (x86 * x631) + (-1 * x52 * x648) + x584 + x650;
	const FLT x652 = x607 * x582;
	const FLT x653 = x62 * x606;
	const FLT x654 = -1 * x79;
	const FLT x655 = (-1 * x604 * x644) + x594 + (x641 * x387) + x654 + (-1 * x63 * x546) + (x48 * x648) + (x602 * x637) + x652 + (x404 * x639) + (-1 * x643 * x398) + (-1 * x653);
	const FLT x656 = (x582 * x587) + (-1 * x62 * x547);
	const FLT x657 = x656 + x614 + (-1 * x403 * x643) + (-1 * x418 * x641) + (-1 * x87 * x644) + (x93 * x631) + x80 + (-1 * x632 * x580) + (-1 * x433 * x638) + (x640 * x387);
	const FLT x658 = (x657 * sensor_pt[1]) + (-1 * x651 * sensor_pt[0]) + (x655 * sensor_pt[2]);
	const FLT x659 = (x647 * sensor_pt[0]) + (-1 * x657 * sensor_pt[2]) + (x655 * sensor_pt[1]);
	const FLT x660 = (x442 * x651) + (-1 * x414 * x647) + (-1 * x441 * x659) + (x439 * x658);
	const FLT x661 = (x651 * sensor_pt[2]) + (-1 * x647 * sensor_pt[1]) + (x655 * sensor_pt[0]);
	const FLT x662 = (x441 * x661) + (-1 * x445 * x658) + (-1 * x442 * x657) + (x446 * x647);
	const FLT x663 = (-1 * x439 * x661) + (x445 * x659) + (-1 * x446 * x651) + (x414 * x657);
	const FLT x664 = (-1 * x41 * x663) + (x444 * x660) + (x39 * x662);
	const FLT x665 = x32 * x663;
	const FLT x666 = (-1 * x39 * x660) + (x10 * x665) + (x444 * x662);
	const FLT x667 = x663 + (x664 * x209) + (-1 * x666 * x193);
	const FLT x668 = (-1 * x662 * x108) + (x41 * x660) + (x6 * x665);
	const FLT x669 = x660 + (x666 * x219) + (-1 * x668 * x209);
	const FLT x670 = x662 + (-1 * x664 * x219) + (x668 * x193);
	const FLT x671 = (-1 * x137 * ((x670 * x135) + (-1 * ((x667 * x123) + (x669 * x122)) * x126))) + (-1 * ((-1 * x669 * x118) + (x667 * x113)) * x121);
	const FLT x672 = x57 * x581;
	const FLT x673 = x75 * x672;
	const FLT x674 = x44 * x576;
	const FLT x675 = x60 * x44;
	const FLT x676 = x44 * x44 * x44;
	const FLT x677 = (x44 * x565) + (-1 * x676 * x556) + (-1 * x675 * x556) + (x675 * x560) + (-1 * x44 * x567) + (x44 * x564) + (x676 * x560) + (-1 * x44 * x562);
	const FLT x678 = x677 * x570;
	const FLT x679 = x677 * x578;
	const FLT x680 = 1.0/2.0 * x679;
	const FLT x681 = x677 * x573;
	const FLT x682 = x418 * x677;
	const FLT x683 = x598 + (-1 * x682 * x571) + (-1 * x53 * x680) + (-1 * x433 * x678) + (-1 * x652) + x654 + (x681 * x387) + (-1 * x57 * x546) + (x48 * x673) + x653 + (-1 * x87 * x674);
	const FLT x684 = x677 * x571;
	const FLT x685 = x57 * x545;
	const FLT x686 = x59 * x678;
	const FLT x687 = (x404 * x684) + x548 + (-1 * x674 * x580) + (-1 * x52 * x673) + (-1 * x681 * x399) + (x86 * x685) + x649 + (-1 * x650) + x84 + (-1 * x589) + (-1 * x686 * x387) + (-1 * x679 * x395);
	const FLT x688 = x656 + (-1 * x597 * x576) + (x404 * x686) + (x672 * x580) + (-1 * x613) + (x684 * x387) + x610 + (-1 * x93 * x685) + (x682 * x573) + (-1 * x52 * x680) + x612;
	const FLT x689 = x646 + x608 + (-1 * x87 * x672) + (-1 * x48 * x680) + (-1 * x576 * x587) + (x419 * x678) + (-1 * x404 * x681) + (-1 * x684 * x399) + x88 + (x83 * x685);
	const FLT x690 = (x683 * sensor_pt[1]) + (x688 * sensor_pt[2]) + (-1 * x689 * sensor_pt[0]);
	const FLT x691 = (x689 * sensor_pt[2]) + (-1 * x687 * sensor_pt[1]) + (x688 * sensor_pt[0]);
	const FLT x692 = (x441 * x691) + (-1 * x445 * x690) + (-1 * x442 * x683) + (x446 * x687);
	const FLT x693 = (x687 * sensor_pt[0]) + (-1 * x683 * sensor_pt[2]) + (x688 * sensor_pt[1]);
	const FLT x694 = (x439 * x690) + (x442 * x689) + (-1 * x414 * x687) + (-1 * x441 * x693);
	const FLT x695 = (-1 * x439 * x691) + (x445 * x693) + (-1 * x446 * x689) + (x414 * x683);
	const FLT x696 = (-1 * x41 * x695) + (x39 * x692) + (x444 * x694);
	const FLT x697 = x32 * ((-1 * x39 * x694) + (x695 * x108) + (x444 * x692));
	const FLT x698 = (x696 * x209) + x695 + (-1 * x19 * x697);
	const FLT x699 = (-1 * x692 * x108) + (x41 * x694) + (x444 * x695);
	const FLT x700 = x694 + (x697 * x127) + (-1 * x699 * x209);
	const FLT x701 = x692 + (x699 * x193) + (-1 * x696 * x219);
	const FLT x702 = (-1 * x137 * ((x701 * x135) + (-1 * ((x698 * x123) + (x700 * x122)) * x126))) + (-1 * ((-1 * x700 * x118) + (x698 * x113)) * x121);
	const FLT x703 = dt * x26;
	const FLT x704 = dt * x22;
	const FLT x705 = x704 + x703;
	const FLT x706 = -1 * dt * x115;
	const FLT x707 = -1 * dt * x116;
	const FLT x708 = x707 + x706 + dt;
	const FLT x709 = dt * x131;
	const FLT x710 = dt * x130;
	const FLT x711 = (-1 * x710) + x709;
	const FLT x712 = (-1 * x137 * ((x711 * x135) + (-1 * ((x705 * x123) + (x708 * x122)) * x126))) + (-1 * ((-1 * x708 * x118) + (x705 * x113)) * x121);
	const FLT x713 = dt * x146;
	const FLT x714 = dt * x145;
	const FLT x715 = (-1 * x714) + x713;
	const FLT x716 = x709 + x710;
	const FLT x717 = (-1 * dt * x151) + dt;
	const FLT x718 = x717 + x707;
	const FLT x719 = (-1 * x137 * ((x718 * x135) + (-1 * ((x715 * x123) + (x716 * x122)) * x126))) + (-1 * ((-1 * x716 * x118) + (x715 * x113)) * x121);
	const FLT x720 = x717 + x706;
	const FLT x721 = (-1 * x703) + x704;
	const FLT x722 = x713 + x714;
	const FLT x723 = (-1 * x137 * ((x722 * x135) + (-1 * ((x720 * x123) + (x721 * x122)) * x126))) + (-1 * ((-1 * x721 * x118) + (x720 * x113)) * x121);
	const FLT x724 = x124 * x134 * x137;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), x138 + (x138 * x141) + (x144 * ((-1 * x118 * x133) + (x28 * x142))));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), x154 + (x141 * x154) + (((-1 * x118 * x153) + (x142 * x148)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), x159 + (x141 * x159) + (((-1 * x118 * x158) + (x142 * x155)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x248 + (x248 * x141) + (((-1 * x247 * x118) + (x218 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x291 + (x291 * x141) + (((-1 * x290 * x118) + (x280 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x320 + (x320 * x141) + (((-1 * x319 * x118) + (x312 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x345 + (x345 * x141) + (((-1 * x344 * x118) + (x340 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), x359 + (x359 * x141) + (((-1 * x358 * x118) + (x351 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), x366 + (x366 * x141) + (((-1 * x365 * x118) + (x362 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), x370 + (x370 * x141) + (((-1 * x369 * x118) + (x367 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), (x376 * x141) + x376 + (((-1 * x375 * x118) + (x371 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), x381 + (x381 * x141) + (((-1 * x380 * x118) + (x377 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), x385 + (x385 * x141) + (((-1 * x384 * x118) + (x382 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x457 + (x457 * x141) + (((-1 * x456 * x118) + (x453 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x491 + (x491 * x141) + (((-1 * x490 * x118) + (x487 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x518 + (x518 * x141) + (((-1 * x517 * x118) + (x514 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x543 + (x543 * x141) + (((-1 * x542 * x118) + (x539 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x630 + (x630 * x141) + (((-1 * x629 * x118) + (x626 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x671 + (x671 * x141) + (((-1 * x670 * x118) + (x667 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), (x702 * x141) + x702 + (((-1 * x701 * x118) + (x698 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), x712 + (x712 * x141) + (((-1 * x711 * x118) + (x705 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), (x719 * x141) + x719 + (((-1 * x718 * x118) + (x715 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), (x723 * x141) + x723 + (((-1 * x722 * x118) + (x720 * x142)) * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), x143 * x143);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), -1 * cos(x140));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), x141);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), -1 + (-1 * x141));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), (-1 * x724 * x141) + (-1 * x724));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]

static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_x0(Hx, dt, _x0, error_model, sensor_pt);
    }
}
// Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_error_model(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x1 = 0.5 * (*_x0).Lighthouse.Rot[0];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x3 = (x1 * (*error_model).Lighthouse.AxisAngleRot[1]) + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (x0 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2];
	const FLT x4 = dt * fabs(dt);
	const FLT x5 = 1.0/2.0 * x4;
	const FLT x6 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x7 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x8 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x9 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x10 = (*_x0).Object.Pose.Rot[3] + (x9 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x8 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x7 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x11 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x12 = (-1 * x9 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x7 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x11 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2];
	const FLT x13 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x14 = (-1 * x9 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x11 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x13 * (*_x0).Object.Pose.Rot[2]);
	const FLT x15 = (*_x0).Object.Pose.Rot[1] + (x7 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x13 * (*_x0).Object.Pose.Rot[3]) + (x8 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x16 = (x15 * x15) + (x14 * x14) + (x10 * x10) + (x12 * x12);
	const FLT x17 = 1. / sqrt(x16);
	const FLT x18 = dt * dt;
	const FLT x19 = x6 * x6;
	const FLT x20 = x19 * x18;
	const FLT x21 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x22 = x21 * x21;
	const FLT x23 = x22 * x18;
	const FLT x24 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x25 = x24 * x24;
	const FLT x26 = x25 * x18;
	const FLT x27 = 1e-10 + x26 + x20 + x23;
	const FLT x28 = sqrt(x27);
	const FLT x29 = 0.5 * x28;
	const FLT x30 = sin(x29);
	const FLT x31 = x30 * (1. / x28);
	const FLT x32 = dt * x31;
	const FLT x33 = x32 * x17;
	const FLT x34 = x30 * x30;
	const FLT x35 = 1. / x27;
	const FLT x36 = x34 * x35;
	const FLT x37 = cos(x29);
	const FLT x38 = (x36 * x23) + (x37 * x37) + (x36 * x20) + (x36 * x26);
	const FLT x39 = 1. / sqrt(x38);
	const FLT x40 = x39 * x14;
	const FLT x41 = x40 * x33;
	const FLT x42 = x39 * x15;
	const FLT x43 = x42 * x33;
	const FLT x44 = x37 * x17;
	const FLT x45 = x39 * x10;
	const FLT x46 = x44 * x45;
	const FLT x47 = x39 * x12;
	const FLT x48 = x47 * x33;
	const FLT x49 = x46 + (x48 * x21) + (x6 * x41) + (-1 * x43 * x24);
	const FLT x50 = x45 * x17;
	const FLT x51 = x50 * x32;
	const FLT x52 = x44 * x39;
	const FLT x53 = x52 * x14;
	const FLT x54 = (-1 * x43 * x21) + (-1 * x6 * x51) + x53 + (-1 * x48 * x24);
	const FLT x55 = x52 * x15;
	const FLT x56 = (x41 * x21) + x55 + (-1 * x6 * x48) + (x51 * x24);
	const FLT x57 = (-1 * x56 * sensor_pt[2]) + (x54 * sensor_pt[1]) + (x49 * sensor_pt[0]);
	const FLT x58 = x52 * x12;
	const FLT x59 = (x6 * x43) + (x41 * x24) + (-1 * x51 * x21) + x58;
	const FLT x60 = (-1 * x59 * sensor_pt[0]) + (x54 * sensor_pt[2]) + (x56 * sensor_pt[1]);
	const FLT x61 = (*_x0).Object.Pose.Pos[0] + (x5 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0] + (2 * ((x60 * x59) + (-1 * x57 * x49)));
	const FLT x62 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x63 = (*_x0).Lighthouse.Rot[0] + (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (-1 * x62 * (*_x0).Lighthouse.Rot[2]);
	const FLT x64 = (x62 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x1 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x0 * (*_x0).Lighthouse.Rot[2]);
	const FLT x65 = x64 * x64;
	const FLT x66 = x63 * x63;
	const FLT x67 = x3 * x3;
	const FLT x68 = (*_x0).Lighthouse.Rot[1] + (x0 * (*_x0).Lighthouse.Rot[0]) + (-1 * x62 * (*_x0).Lighthouse.Rot[3]) + (x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x69 = x68 * x68;
	const FLT x70 = x69 + x67 + x65 + x66;
	const FLT x71 = 1. / x70;
	const FLT x72 = x71 * x65;
	const FLT x73 = x71 * x67;
	const FLT x74 = x71 * x69;
	const FLT x75 = x74 + (x71 * x66) + x72 + x73;
	const FLT x76 = 1. / sqrt(x75);
	const FLT x77 = 1. / sqrt(x70);
	const FLT x78 = x77 * x76;
	const FLT x79 = x78 * x63;
	const FLT x80 = (-1 * x49 * sensor_pt[1]) + (x54 * sensor_pt[0]) + (x59 * sensor_pt[2]);
	const FLT x81 = (*_x0).Object.Pose.Pos[2] + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (2 * ((x57 * x56) + (-1 * x80 * x59))) + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x5 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x82 = x3 * x78;
	const FLT x83 = (*_x0).Object.Pose.Pos[1] + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (x5 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (2 * ((x80 * x49) + (-1 * x60 * x56)));
	const FLT x84 = x78 * x64;
	const FLT x85 = (x83 * x84) + (x79 * x61) + (-1 * x81 * x82);
	const FLT x86 = x3 * x85;
	const FLT x87 = 1.0 * x68;
	const FLT x88 = x87 * (*_x0).Lighthouse.Rot[0];
	const FLT x89 = 1.0 * x3;
	const FLT x90 = x89 * (*_x0).Lighthouse.Rot[3];
	const FLT x91 = 1.0 * x64;
	const FLT x92 = x91 * (*_x0).Lighthouse.Rot[2];
	const FLT x93 = 1.0 * x63;
	const FLT x94 = x93 * (*_x0).Lighthouse.Rot[1];
	const FLT x95 = (-1 * x94) + (-1 * x92) + x88 + x90;
	const FLT x96 = (1. / (x70 * sqrt(x70))) * x76;
	const FLT x97 = x96 * x95;
	const FLT x98 = 1.0/2.0 * x83;
	const FLT x99 = x64 * x97;
	const FLT x100 = 1.0/2.0 * x61;
	const FLT x101 = x63 * x97;
	const FLT x102 = 1.0/2.0 * x81;
	const FLT x103 = x97 * x102;
	const FLT x104 = 1. / (x70 * x70);
	const FLT x105 = x95 * x104;
	const FLT x106 = (1. / (x75 * sqrt(x75))) * x77;
	const FLT x107 = ((-1 * x71 * x94) + (-1 * x67 * x105) + (-1 * x71 * x92) + (x88 * x71) + (x71 * x90) + (-1 * x66 * x105) + (-1 * x65 * x105) + (-1 * x69 * x105)) * x106;
	const FLT x108 = x63 * x107;
	const FLT x109 = x98 * x107;
	const FLT x110 = x3 * x107;
	const FLT x111 = 0.5 * x78;
	const FLT x112 = x61 * x111;
	const FLT x113 = x81 * x78;
	const FLT x114 = 0.5 * x113;
	const FLT x115 = x83 * x111;
	const FLT x116 = (-1 * x115 * (*_x0).Lighthouse.Rot[2]) + (-1 * x112 * (*_x0).Lighthouse.Rot[1]) + (-1 * x114 * (*_x0).Lighthouse.Rot[3]);
	const FLT x117 = (x102 * x110) + (-1 * x101 * x100) + (-1 * x99 * x98) + x116 + (x3 * x103) + (-1 * x64 * x109) + (-1 * x100 * x108);
	const FLT x118 = 2 * x82;
	const FLT x119 = x78 * x68;
	const FLT x120 = (x81 * x119) + (x83 * x79) + (-1 * x84 * x61);
	const FLT x121 = x68 * x120;
	const FLT x122 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x123 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x124 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x125 = (x119 * x124) + (x79 * x122) + (-1 * x84 * x123);
	const FLT x126 = x107 * x125;
	const FLT x127 = x97 * x125;
	const FLT x128 = 1.0/2.0 * x122;
	const FLT x129 = x64 * x107;
	const FLT x130 = 1.0/2.0 * x123;
	const FLT x131 = 1.0/2.0 * x124;
	const FLT x132 = x97 * x128;
	const FLT x133 = x97 * x131;
	const FLT x134 = x111 * x123;
	const FLT x135 = x111 * (*_x0).Lighthouse.Rot[2];
	const FLT x136 = x111 * (*_x0).Lighthouse.Rot[3];
	const FLT x137 = (-1 * x124 * x136) + (-1 * x134 * (*_x0).Lighthouse.Rot[1]) + (-1 * x122 * x135);
	const FLT x138 = x137 + (-1 * x101 * x130) + (-1 * x108 * x130) + (-1 * x128 * x129) + (x110 * x131) + (-1 * x64 * x132) + (x3 * x133);
	const FLT x139 = (x84 * x122) + (-1 * x82 * x124) + (x79 * x123);
	const FLT x140 = x107 * x139;
	const FLT x141 = x97 * x139;
	const FLT x142 = 1.0 * x78;
	const FLT x143 = x85 * x142;
	const FLT x144 = x120 * x142;
	const FLT x145 = x144 * (*_x0).Lighthouse.Rot[0];
	const FLT x146 = x125 * x142;
	const FLT x147 = x146 * (*_x0).Lighthouse.Rot[0];
	const FLT x148 = x1 * x113;
	const FLT x149 = x115 * (*_x0).Lighthouse.Rot[1];
	const FLT x150 = x112 * (*_x0).Lighthouse.Rot[2];
	const FLT x151 = x68 * x107;
	const FLT x152 = (-1 * x102 * x151) + (x100 * x129) + x148 + (-1 * x68 * x103) + (-1 * x63 * x109) + (-1 * x149) + x150 + (x99 * x100) + (-1 * x98 * x101);
	const FLT x153 = 2 * x119;
	const FLT x154 = x1 * x78;
	const FLT x155 = x124 * x154;
	const FLT x156 = x111 * (*_x0).Lighthouse.Rot[1];
	const FLT x157 = x122 * x156;
	const FLT x158 = x134 * (*_x0).Lighthouse.Rot[2];
	const FLT x159 = x158 + (x129 * x130) + (-1 * x157) + (x99 * x130) + (-1 * x68 * x133) + x155 + (-1 * x108 * x128) + (-1 * x101 * x128) + (-1 * x131 * x151);
	const FLT x160 = x139 * x142;
	const FLT x161 = (x153 * x159) + (-1 * x152 * x153) + (-1 * x86 * x107) + (-1 * x68 * x127) + (x3 * x141) + (-1 * x160 * (*_x0).Lighthouse.Rot[3]) + (x97 * x121) + (x107 * x121) + x147 + (-1 * x68 * x126) + (-1 * x86 * x97) + (x3 * x140) + (x118 * x117) + (-1 * x118 * x138) + (x143 * (*_x0).Lighthouse.Rot[3]) + (-1 * x145);
	const FLT x162 = (x82 * x123) + (x79 * x124) + (-1 * x119 * x122);
	const FLT x163 = (x82 * x61) + (x81 * x79) + (-1 * x83 * x119);
	const FLT x164 = x61 + (-1 * (x123 + (2 * ((-1 * x82 * x162) + (x84 * x125))))) + (2 * ((-1 * x82 * x163) + (x84 * x120)));
	const FLT x165 = (-1 * (x124 + (2 * ((-1 * x119 * x125) + (x82 * x139))))) + x81 + (2 * ((-1 * x119 * x120) + (x82 * x85)));
	const FLT x166 = x165 * x165;
	const FLT x167 = 1. / x166;
	const FLT x168 = x167 * x164;
	const FLT x169 = x3 * x163;
	const FLT x170 = x112 * (*_x0).Lighthouse.Rot[3];
	const FLT x171 = x114 * (*_x0).Lighthouse.Rot[1];
	const FLT x172 = x83 * x154;
	const FLT x173 = (-1 * x172) + (x68 * x109) + (-1 * x102 * x108) + x170 + (-1 * x100 * x110) + (-1 * x3 * x97 * x100) + (-1 * x101 * x102) + (x68 * x98 * x97) + (-1 * x171);
	const FLT x174 = x64 * x120;
	const FLT x175 = x3 * x162;
	const FLT x176 = 2 * x84;
	const FLT x177 = x134 * (*_x0).Lighthouse.Rot[3];
	const FLT x178 = x124 * x156;
	const FLT x179 = x122 * x154;
	const FLT x180 = x3 * x130;
	const FLT x181 = (-1 * x179) + (-1 * x97 * x180) + (-1 * x178) + (-1 * x108 * x131) + x177 + (-1 * x63 * x133) + (x128 * x151) + (-1 * x110 * x130) + (x68 * x132);
	const FLT x182 = (x146 * (*_x0).Lighthouse.Rot[2]) + (-1 * x144 * (*_x0).Lighthouse.Rot[2]);
	const FLT x183 = x163 * x142;
	const FLT x184 = x162 * x142;
	const FLT x185 = (x184 * (*_x0).Lighthouse.Rot[3]) + (-1 * x183 * (*_x0).Lighthouse.Rot[3]);
	const FLT x186 = x182 + (-1 * x97 * x175) + x185 + (-1 * x107 * x175) + (x64 * x127) + (-1 * x97 * x174) + (x107 * x169) + (-1 * x118 * x173) + (x176 * x152) + (-1 * x176 * x159) + (x97 * x169) + (x118 * x181) + (-1 * x107 * x174) + (x64 * x126);
	const FLT x187 = 1. / x165;
	const FLT x188 = (x164 * x164) + x166;
	const FLT x189 = 1. / x188;
	const FLT x190 = x166 * x189;
	const FLT x191 = x83 + (2 * ((-1 * x84 * x85) + (x119 * x163))) + (-1 * (x122 + (2 * ((-1 * x84 * x139) + (x119 * x162)))));
	const FLT x192 = x191 * x191;
	const FLT x193 = (*error_model).BSD0.tilt + (*_x0).BSD0.tilt;
	const FLT x194 = 1. / sqrt(1 + (-1 * x189 * x192 * (x193 * x193)));
	const FLT x195 = 2 * x164;
	const FLT x196 = 2 * x165;
	const FLT x197 = 1.0/2.0 * (1. / (x188 * sqrt(x188))) * x191 * x193;
	const FLT x198 = x183 * (*_x0).Lighthouse.Rot[0];
	const FLT x199 = x85 * x64;
	const FLT x200 = x68 * x163;
	const FLT x201 = x68 * x162;
	const FLT x202 = x184 * (*_x0).Lighthouse.Rot[0];
	const FLT x203 = (x97 * x201) + (-1 * x117 * x176) + (-1 * x200 * x107) + (-1 * x202) + x198 + (-1 * x181 * x153) + (-1 * x97 * x200) + (x173 * x153) + (-1 * x64 * x141) + (-1 * x64 * x140) + (x176 * x138) + (-1 * x160 * (*_x0).Lighthouse.Rot[2]) + (x107 * x199) + (x97 * x199) + (x143 * (*_x0).Lighthouse.Rot[2]) + (x201 * x107);
	const FLT x204 = 1. / sqrt(x188);
	const FLT x205 = x204 * x193;
	const FLT x206 = (-1 * x194 * ((x203 * x205) + (-1 * ((x161 * x196) + (x186 * x195)) * x197))) + (-1 * ((-1 * x187 * x186) + (x161 * x168)) * x190);
	const FLT x207 = -1 * x165;
	const FLT x208 = 1.5707963267949 + (*_x0).BSD0.gibpha + (*error_model).BSD0.gibpha + (-1 * atan2(x164, x207)) + (-1 * ((*error_model).BSD0.phase + (*_x0).BSD0.phase)) + (-1 * asin(x205 * x191));
	const FLT x209 = sin(x208) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag);
	const FLT x210 = x167 * x191;
	const FLT x211 = atan2(x191, x207);
	const FLT x212 = 2 * (1. / (x166 + x192)) * x211 * x166 * ((*error_model).BSD0.curve + (*_x0).BSD0.curve);
	const FLT x213 = x87 * (*_x0).Lighthouse.Rot[3];
	const FLT x214 = x89 * (*_x0).Lighthouse.Rot[0];
	const FLT x215 = x91 * (*_x0).Lighthouse.Rot[1];
	const FLT x216 = x93 * (*_x0).Lighthouse.Rot[2];
	const FLT x217 = (-1 * x216) + x215 + (-1 * x213) + x214;
	const FLT x218 = x217 * x104;
	const FLT x219 = ((-1 * x67 * x218) + (x71 * x214) + (x71 * x215) + (-1 * x65 * x218) + (-1 * x69 * x218) + (-1 * x71 * x213) + (-1 * x71 * x216) + (-1 * x66 * x218)) * x106;
	const FLT x220 = x143 * (*_x0).Lighthouse.Rot[0];
	const FLT x221 = x96 * x217;
	const FLT x222 = x64 * x221;
	const FLT x223 = x98 * x219;
	const FLT x224 = x3 * x221;
	const FLT x225 = x63 * x221;
	const FLT x226 = x219 * x102;
	const FLT x227 = x219 * x100;
	const FLT x228 = (-1 * x63 * x227) + (x3 * x226) + (-1 * x98 * x222) + (-1 * x148) + (-1 * x64 * x223) + x149 + (x224 * x102) + (-1 * x150) + (-1 * x225 * x100);
	const FLT x229 = x221 * x125;
	const FLT x230 = x68 * x221;
	const FLT x231 = x64 * x219;
	const FLT x232 = (x231 * x100) + (-1 * x63 * x223) + x116 + (x222 * x100) + (-1 * x230 * x102) + (-1 * x98 * x225) + (-1 * x68 * x226);
	const FLT x233 = x221 * x139;
	const FLT x234 = x160 * (*_x0).Lighthouse.Rot[0];
	const FLT x235 = x219 * x131;
	const FLT x236 = x219 * x128;
	const FLT x237 = (x222 * x130) + (-1 * x230 * x131) + (x231 * x130) + (-1 * x68 * x235) + (-1 * x63 * x236) + x137 + (-1 * x225 * x128);
	const FLT x238 = x219 * x125;
	const FLT x239 = x219 * x139;
	const FLT x240 = (-1 * x63 * x219 * x130) + (x224 * x131) + x157 + (-1 * x158) + (-1 * x222 * x128) + (-1 * x225 * x130) + (x3 * x235) + (-1 * x155) + (-1 * x231 * x128);
	const FLT x241 = x221 * x120;
	const FLT x242 = (-1 * x86 * x221) + (x3 * x239) + (-1 * x68 * x229) + (-1 * x146 * (*_x0).Lighthouse.Rot[3]) + (-1 * x234) + (x144 * (*_x0).Lighthouse.Rot[3]) + (x228 * x118) + (x237 * x153) + (-1 * x86 * x219) + (x219 * x121) + (x3 * x233) + (x68 * x241) + (-1 * x240 * x118) + x220 + (-1 * x68 * x238) + (-1 * x232 * x153);
	const FLT x243 = x124 * x135;
	const FLT x244 = x122 * x136;
	const FLT x245 = x123 * x154;
	const FLT x246 = x245 + (-1 * x225 * x131) + x244 + (-1 * x219 * x180) + (-1 * x224 * x130) + (x230 * x128) + (x68 * x236) + (-1 * x63 * x235) + (-1 * x243);
	const FLT x247 = x61 * x154;
	const FLT x248 = x114 * (*_x0).Lighthouse.Rot[2];
	const FLT x249 = x115 * (*_x0).Lighthouse.Rot[3];
	const FLT x250 = (-1 * x225 * x102) + (x68 * x223) + x249 + (-1 * x224 * x100) + (-1 * x3 * x227) + x247 + (x98 * x230) + (-1 * x248) + (-1 * x63 * x226);
	const FLT x251 = x219 * x163;
	const FLT x252 = (x3 * x251) + (x64 * x238) + (-1 * x146 * (*_x0).Lighthouse.Rot[1]) + (x144 * (*_x0).Lighthouse.Rot[1]) + (-1 * x221 * x175) + (-1 * x198) + (x64 * x229) + (x232 * x176) + (-1 * x219 * x175) + (x246 * x118) + (-1 * x219 * x174) + (x221 * x169) + (-1 * x237 * x176) + x202 + (-1 * x64 * x241) + (-1 * x250 * x118);
	const FLT x253 = (x160 * (*_x0).Lighthouse.Rot[1]) + (-1 * x143 * (*_x0).Lighthouse.Rot[1]);
	const FLT x254 = (x219 * x201) + (-1 * x200 * x221) + x185 + (x250 * x153) + (-1 * x64 * x233) + x253 + (-1 * x228 * x176) + (x201 * x221) + (x221 * x199) + (-1 * x246 * x153) + (-1 * x64 * x239) + (x219 * x199) + (-1 * x68 * x251) + (x240 * x176);
	const FLT x255 = (-1 * x194 * ((x205 * x254) + (-1 * ((x242 * x196) + (x252 * x195)) * x197))) + (-1 * ((-1 * x252 * x187) + (x242 * x168)) * x190);
	const FLT x256 = x87 * (*_x0).Lighthouse.Rot[2];
	const FLT x257 = x89 * (*_x0).Lighthouse.Rot[1];
	const FLT x258 = x91 * (*_x0).Lighthouse.Rot[0];
	const FLT x259 = x93 * (*_x0).Lighthouse.Rot[3];
	const FLT x260 = x258 + (-1 * x259) + x256 + (-1 * x257);
	const FLT x261 = x260 * x104;
	const FLT x262 = ((-1 * x71 * x259) + (x71 * x256) + (-1 * x71 * x257) + (-1 * x67 * x261) + (-1 * x65 * x261) + (-1 * x66 * x261) + (-1 * x69 * x261) + (x71 * x258)) * x106;
	const FLT x263 = x64 * x262;
	const FLT x264 = x96 * x260;
	const FLT x265 = x3 * x264;
	const FLT x266 = x63 * x262;
	const FLT x267 = x64 * x264;
	const FLT x268 = x63 * x264;
	const FLT x269 = x3 * x262;
	const FLT x270 = (x269 * x102) + (-1 * x170) + (-1 * x268 * x100) + x171 + (-1 * x266 * x100) + (x265 * x102) + x172 + (-1 * x98 * x263) + (-1 * x98 * x267);
	const FLT x271 = x264 * x139;
	const FLT x272 = (x265 * x131) + (-1 * x263 * x128) + x178 + (x269 * x131) + x179 + (-1 * x177) + (-1 * x268 * x130) + (-1 * x266 * x130) + (-1 * x267 * x128);
	const FLT x273 = x262 * x120;
	const FLT x274 = x68 * x262;
	const FLT x275 = x68 * x264;
	const FLT x276 = x243 + (x263 * x130) + (-1 * x266 * x128) + (-1 * x268 * x128) + (-1 * x245) + (-1 * x274 * x131) + (-1 * x244) + (x267 * x130) + (-1 * x275 * x131);
	const FLT x277 = x264 * x125;
	const FLT x278 = x262 * x139;
	const FLT x279 = x262 * x125;
	const FLT x280 = x264 * x120;
	const FLT x281 = (-1 * x98 * x268) + (-1 * x98 * x266) + (-1 * x249) + (x267 * x100) + (-1 * x275 * x102) + (-1 * x274 * x102) + (-1 * x247) + (x263 * x100) + x248;
	const FLT x282 = (-1 * x281 * x153) + (-1 * x272 * x118) + (x68 * x273) + (x3 * x271) + (-1 * x86 * x262) + x182 + (-1 * x86 * x264) + (x270 * x118) + (x276 * x153) + (-1 * x68 * x277) + x253 + (x68 * x280) + (x3 * x278) + (-1 * x68 * x279);
	const FLT x283 = x116 + (x98 * x275) + (-1 * x269 * x100) + (x98 * x274) + (-1 * x268 * x102) + (-1 * x266 * x102) + (-1 * x265 * x100);
	const FLT x284 = x137 + (-1 * x269 * x130) + (-1 * x268 * x131) + (-1 * x265 * x130) + (x275 * x128) + (x274 * x128) + (-1 * x266 * x131);
	const FLT x285 = (x64 * x277) + (-1 * x147) + x145 + (x264 * x169) + (x281 * x176) + (-1 * x283 * x118) + (x183 * (*_x0).Lighthouse.Rot[1]) + (-1 * x64 * x280) + (-1 * x264 * x175) + (x262 * x169) + (-1 * x262 * x175) + (x64 * x279) + (-1 * x184 * (*_x0).Lighthouse.Rot[1]) + (x284 * x118) + (-1 * x64 * x273) + (-1 * x276 * x176);
	const FLT x286 = (-1 * x200 * x264) + (x262 * x199) + (-1 * x270 * x176) + (-1 * x184 * (*_x0).Lighthouse.Rot[2]) + (x201 * x262) + (x183 * (*_x0).Lighthouse.Rot[2]) + (x264 * x199) + (-1 * x284 * x153) + (x272 * x176) + (x201 * x264) + x234 + (-1 * x200 * x262) + (-1 * x220) + (-1 * x64 * x278) + (-1 * x64 * x271) + (x283 * x153);
	const FLT x287 = (-1 * x194 * ((x205 * x286) + (-1 * ((x282 * x196) + (x285 * x195)) * x197))) + (-1 * ((-1 * x285 * x187) + (x282 * x168)) * x190);
	const FLT x288 = 1. / x75;
	const FLT x289 = 2 * x288;
	const FLT x290 = x71 * x289;
	const FLT x291 = x64 * x68;
	const FLT x292 = x291 * x290;
	const FLT x293 = -1 * x292;
	const FLT x294 = x63 * x290;
	const FLT x295 = x3 * x294;
	const FLT x296 = -1 * x295;
	const FLT x297 = x296 + x293;
	const FLT x298 = x73 * x289;
	const FLT x299 = x72 * x289;
	const FLT x300 = -1 + x299 + x298;
	const FLT x301 = x64 * x294;
	const FLT x302 = x3 * x290;
	const FLT x303 = x68 * x302;
	const FLT x304 = -1 * x303;
	const FLT x305 = x304 + x301;
	const FLT x306 = (-1 * x194 * ((x205 * x305) + (-1 * ((x297 * x196) + (x300 * x195)) * x197))) + (-1 * ((-1 * x300 * x187) + (x297 * x168)) * x190);
	const FLT x307 = x68 * x294;
	const FLT x308 = x64 * x302;
	const FLT x309 = -1 * x308;
	const FLT x310 = x309 + x307;
	const FLT x311 = -1 * x301;
	const FLT x312 = x311 + x304;
	const FLT x313 = x74 * x289;
	const FLT x314 = -1 + x313;
	const FLT x315 = x314 + x299;
	const FLT x316 = (-1 * x194 * ((x205 * x315) + (-1 * ((x310 * x196) + (x312 * x195)) * x197))) + (-1 * ((-1 * x312 * x187) + (x310 * x168)) * x190);
	const FLT x317 = x314 + x298;
	const FLT x318 = x293 + x295;
	const FLT x319 = -1 * x307;
	const FLT x320 = x319 + x309;
	const FLT x321 = (-1 * x194 * ((x205 * x320) + (-1 * ((x317 * x196) + (x318 * x195)) * x197))) + (-1 * ((-1 * x318 * x187) + (x317 * x168)) * x190);
	const FLT x322 = x4 * x288;
	const FLT x323 = x71 * x322;
	const FLT x324 = x3 * x323;
	const FLT x325 = x63 * x324;
	const FLT x326 = x291 * x323;
	const FLT x327 = x326 + x325;
	const FLT x328 = -1 * x73 * x322;
	const FLT x329 = (-1 * x72 * x322) + x5;
	const FLT x330 = x329 + x328;
	const FLT x331 = x68 * x323;
	const FLT x332 = x3 * x331;
	const FLT x333 = x63 * x64 * x323;
	const FLT x334 = (-1 * x333) + x332;
	const FLT x335 = (-1 * x194 * ((x205 * x334) + (-1 * ((x327 * x196) + (x330 * x195)) * x197))) + (-1 * ((-1 * x330 * x187) + (x327 * x168)) * x190);
	const FLT x336 = x64 * x324;
	const FLT x337 = x63 * x331;
	const FLT x338 = (-1 * x337) + x336;
	const FLT x339 = x332 + x333;
	const FLT x340 = -1 * x74 * x322;
	const FLT x341 = x329 + x340;
	const FLT x342 = (-1 * x194 * ((x205 * x341) + (-1 * ((x338 * x196) + (x339 * x195)) * x197))) + (-1 * ((-1 * x339 * x187) + (x338 * x168)) * x190);
	const FLT x343 = x340 + x328 + x5;
	const FLT x344 = (-1 * x325) + x326;
	const FLT x345 = x336 + x337;
	const FLT x346 = (-1 * x194 * ((x205 * x345) + (-1 * ((x343 * x196) + (x344 * x195)) * x197))) + (-1 * ((-1 * x344 * x187) + (x343 * x168)) * x190);
	const FLT x347 = 1.0 * x15;
	const FLT x348 = 1.0 * x14;
	const FLT x349 = 1.0 * x10;
	const FLT x350 = 1.0 * (*_x0).Object.Pose.Rot[3];
	const FLT x351 = (x12 * x350) + (-1 * x349 * (*_x0).Object.Pose.Rot[2]) + (x347 * (*_x0).Object.Pose.Rot[0]) + (-1 * x348 * (*_x0).Object.Pose.Rot[1]);
	const FLT x352 = 1. / (x16 * sqrt(x16));
	const FLT x353 = x37 * x352;
	const FLT x354 = x351 * x353;
	const FLT x355 = 1.0/2.0 * x354;
	const FLT x356 = 1.0/2.0 * x14;
	const FLT x357 = x39 * x356;
	const FLT x358 = x32 * x352;
	const FLT x359 = x358 * x357;
	const FLT x360 = x359 * x351;
	const FLT x361 = 1.0/2.0 * x47;
	const FLT x362 = x351 * x358;
	const FLT x363 = x6 * x362;
	const FLT x364 = 1.0/2.0 * x45;
	const FLT x365 = x24 * x362;
	const FLT x366 = x33 * x39;
	const FLT x367 = x6 * x366;
	const FLT x368 = x24 * x366;
	const FLT x369 = x21 * x366;
	const FLT x370 = (-1 * x9 * x369) + (x7 * x52) + (-1 * x11 * x367) + (-1 * x8 * x368);
	const FLT x371 = (-1 * x364 * x365) + (x361 * x363) + x370 + (-1 * x42 * x355) + (-1 * x21 * x360);
	const FLT x372 = 2 * x60;
	const FLT x373 = x21 * x362;
	const FLT x374 = 1.0/2.0 * x42;
	const FLT x375 = x11 * x369;
	const FLT x376 = x7 * x368;
	const FLT x377 = x9 * x367;
	const FLT x378 = x8 * x52;
	const FLT x379 = (-1 * x378) + (-1 * x377) + x375 + (-1 * x376);
	const FLT x380 = x379 + (-1 * x45 * x355) + (x374 * x365) + (-1 * x373 * x361) + (-1 * x6 * x360);
	const FLT x381 = x7 * x369;
	const FLT x382 = x8 * x367;
	const FLT x383 = x11 * x368;
	const FLT x384 = x9 * x52;
	const FLT x385 = (-1 * x384) + (-1 * x383) + (-1 * x381) + x382;
	const FLT x386 = x385 + (x361 * x365) + (x374 * x373) + (x363 * x364) + (-1 * x354 * x357);
	const FLT x387 = x8 * x369;
	const FLT x388 = x9 * x368;
	const FLT x389 = x7 * x367;
	const FLT x390 = x52 * x11;
	const FLT x391 = (-1 * x47 * x355) + x390 + (-1 * x24 * x360) + (x373 * x364) + x387 + x389 + (-1 * x374 * x363) + (-1 * x388);
	const FLT x392 = (x391 * sensor_pt[2]) + (-1 * x380 * sensor_pt[1]) + (x386 * sensor_pt[0]);
	const FLT x393 = 2 * x49;
	const FLT x394 = 2 * x80;
	const FLT x395 = (x371 * sensor_pt[1]) + (-1 * x391 * sensor_pt[0]) + (x386 * sensor_pt[2]);
	const FLT x396 = 2 * x56;
	const FLT x397 = (x394 * x380) + (-1 * x372 * x371) + (-1 * x395 * x396) + (x393 * x392);
	const FLT x398 = 2 * x57;
	const FLT x399 = 2 * x59;
	const FLT x400 = (-1 * x371 * sensor_pt[2]) + (x380 * sensor_pt[0]) + (x386 * sensor_pt[1]);
	const FLT x401 = (x372 * x391) + (-1 * x400 * x393) + (-1 * x398 * x380) + (x399 * x395);
	const FLT x402 = (x371 * x398) + (x400 * x396) + (-1 * x394 * x391) + (-1 * x399 * x392);
	const FLT x403 = (-1 * x82 * x402) + (x84 * x397) + (x79 * x401);
	const FLT x404 = (-1 * x84 * x401) + (x79 * x397) + (x402 * x119);
	const FLT x405 = x402 + (x403 * x118) + (-1 * x404 * x153);
	const FLT x406 = (-1 * x397 * x119) + (x82 * x401) + (x79 * x402);
	const FLT x407 = x401 + (x404 * x176) + (-1 * x406 * x118);
	const FLT x408 = x397 + (x406 * x153) + (-1 * x403 * x176);
	const FLT x409 = (-1 * x194 * ((x408 * x205) + (-1 * ((x405 * x196) + (x407 * x195)) * x197))) + (-1 * ((-1 * x407 * x187) + (x405 * x168)) * x190);
	const FLT x410 = 1.0 * x12;
	const FLT x411 = (x410 * (*_x0).Object.Pose.Rot[0]) + (-1 * x348 * (*_x0).Object.Pose.Rot[2]) + (-1 * x15 * x350) + (x349 * (*_x0).Object.Pose.Rot[1]);
	const FLT x412 = x21 * x411;
	const FLT x413 = x6 * x411;
	const FLT x414 = x413 * x358;
	const FLT x415 = x411 * x353;
	const FLT x416 = 1.0/2.0 * x415;
	const FLT x417 = x364 * x358;
	const FLT x418 = x24 * x411;
	const FLT x419 = x388 + (-1 * x387) + (-1 * x390) + (-1 * x389);
	const FLT x420 = x419 + (-1 * x412 * x359) + (-1 * x417 * x418) + (-1 * x42 * x416) + (x414 * x361);
	const FLT x421 = x361 * x358;
	const FLT x422 = x374 * x358;
	const FLT x423 = (x418 * x422) + (-1 * x382) + x384 + (-1 * x45 * x416) + (-1 * x412 * x421) + (-1 * x413 * x359) + x383 + x381;
	const FLT x424 = (x418 * x421) + x379 + (-1 * x415 * x357) + (x412 * x422) + (x413 * x417);
	const FLT x425 = x370 + (-1 * x415 * x361) + (-1 * x418 * x359) + (x412 * x417) + (-1 * x414 * x374);
	const FLT x426 = (x425 * sensor_pt[2]) + (-1 * x423 * sensor_pt[1]) + (x424 * sensor_pt[0]);
	const FLT x427 = (x420 * sensor_pt[1]) + (-1 * x425 * sensor_pt[0]) + (x424 * sensor_pt[2]);
	const FLT x428 = (x426 * x393) + (-1 * x427 * x396) + (-1 * x420 * x372) + (x423 * x394);
	const FLT x429 = (x423 * sensor_pt[0]) + (-1 * x420 * sensor_pt[2]) + (x424 * sensor_pt[1]);
	const FLT x430 = (x427 * x399) + (x425 * x372) + (-1 * x423 * x398) + (-1 * x429 * x393);
	const FLT x431 = (x420 * x398) + (-1 * x426 * x399) + (-1 * x425 * x394) + (x429 * x396);
	const FLT x432 = (-1 * x82 * x431) + (x84 * x428) + (x79 * x430);
	const FLT x433 = (-1 * x84 * x430) + (x431 * x119) + (x79 * x428);
	const FLT x434 = (x432 * x118) + x431 + (-1 * x433 * x153);
	const FLT x435 = (-1 * x428 * x119) + (x79 * x431) + (x82 * x430);
	const FLT x436 = x430 + (x433 * x176) + (-1 * x435 * x118);
	const FLT x437 = (x435 * x153) + x428 + (-1 * x432 * x176);
	const FLT x438 = (-1 * x194 * ((x437 * x205) + (-1 * ((x434 * x196) + (x436 * x195)) * x197))) + (-1 * ((-1 * x436 * x187) + (x434 * x168)) * x190);
	const FLT x439 = (-1 * x410 * (*_x0).Object.Pose.Rot[1]) + (-1 * x14 * x350) + (x347 * (*_x0).Object.Pose.Rot[2]) + (x349 * (*_x0).Object.Pose.Rot[0]);
	const FLT x440 = x21 * x439;
	const FLT x441 = x439 * x353;
	const FLT x442 = 1.0/2.0 * x441;
	const FLT x443 = x6 * x439;
	const FLT x444 = x24 * x439;
	const FLT x445 = (-1 * x375) + (-1 * x42 * x442) + (-1 * x440 * x359) + (-1 * x417 * x444) + x376 + (x421 * x443) + x377 + x378;
	const FLT x446 = x440 * x358;
	const FLT x447 = 1.0/2.0 * x446;
	const FLT x448 = x444 * x358;
	const FLT x449 = (x448 * x361) + x419 + (x417 * x443) + (-1 * x441 * x357) + (x42 * x447);
	const FLT x450 = x385 + (-1 * x444 * x359) + (-1 * x47 * x442) + (-1 * x422 * x443) + (x45 * x447);
	const FLT x451 = (x445 * sensor_pt[1]) + (x449 * sensor_pt[2]) + (-1 * x450 * sensor_pt[0]);
	const FLT x452 = x370 + (x448 * x374) + (-1 * x443 * x359) + (-1 * x45 * x442) + (-1 * x446 * x361);
	const FLT x453 = (x450 * sensor_pt[2]) + (-1 * x452 * sensor_pt[1]) + (x449 * sensor_pt[0]);
	const FLT x454 = (x453 * x393) + (x452 * x394) + (-1 * x445 * x372) + (-1 * x451 * x396);
	const FLT x455 = (x452 * sensor_pt[0]) + (-1 * x445 * sensor_pt[2]) + (x449 * sensor_pt[1]);
	const FLT x456 = (x450 * x372) + (-1 * x452 * x398) + (x451 * x399) + (-1 * x455 * x393);
	const FLT x457 = (x455 * x396) + (-1 * x450 * x394) + (x445 * x398) + (-1 * x453 * x399);
	const FLT x458 = (-1 * x82 * x457) + (x84 * x454) + (x79 * x456);
	const FLT x459 = (-1 * x84 * x456) + (x457 * x119) + (x79 * x454);
	const FLT x460 = x457 + (x458 * x118) + (-1 * x459 * x153);
	const FLT x461 = (-1 * x454 * x119) + (x82 * x456) + (x79 * x457);
	const FLT x462 = (x459 * x176) + x456 + (-1 * x461 * x118);
	const FLT x463 = (x461 * x153) + x454 + (-1 * x458 * x176);
	const FLT x464 = (-1 * x194 * ((x463 * x205) + (-1 * ((x460 * x196) + (x462 * x195)) * x197))) + (-1 * ((-1 * x462 * x187) + (x460 * x168)) * x190);
	const FLT x465 = x292 + x295;
	const FLT x466 = -1 * x299;
	const FLT x467 = -1 * x298;
	const FLT x468 = 1 + x467 + x466;
	const FLT x469 = x311 + x303;
	const FLT x470 = (-1 * x194 * ((x469 * x205) + (-1 * ((x465 * x196) + (x468 * x195)) * x197))) + (-1 * ((-1 * x468 * x187) + (x465 * x168)) * x190);
	const FLT x471 = x319 + x308;
	const FLT x472 = x303 + x301;
	const FLT x473 = 1 + (-1 * x313);
	const FLT x474 = x473 + x466;
	const FLT x475 = (-1 * x194 * ((x474 * x205) + (-1 * ((x471 * x196) + (x472 * x195)) * x197))) + (-1 * ((-1 * x472 * x187) + (x471 * x168)) * x190);
	const FLT x476 = x473 + x467;
	const FLT x477 = x296 + x292;
	const FLT x478 = x308 + x307;
	const FLT x479 = (-1 * x194 * ((x478 * x205) + (-1 * ((x476 * x196) + (x477 * x195)) * x197))) + (-1 * ((-1 * x477 * x187) + (x476 * x168)) * x190);
	const FLT x480 = dt * dt * dt;
	const FLT x481 = 0.5 * x35 * x480;
	const FLT x482 = x21 * x481;
	const FLT x483 = x6 * x482;
	const FLT x484 = x58 * x483;
	const FLT x485 = x24 * x482;
	const FLT x486 = x46 * x485;
	const FLT x487 = x22 * x481;
	const FLT x488 = 1.0/2.0 * x10;
	const FLT x489 = x21 * x21 * x21;
	const FLT x490 = dt * dt * dt * dt;
	const FLT x491 = 2 * x34 * (1. / (x27 * x27));
	const FLT x492 = x490 * x491;
	const FLT x493 = x30 * (1. / (x27 * sqrt(x27)));
	const FLT x494 = 1.0 * x37;
	const FLT x495 = x494 * x493;
	const FLT x496 = x495 * x490;
	const FLT x497 = x21 * x19;
	const FLT x498 = x25 * x492;
	const FLT x499 = 2 * x36;
	const FLT x500 = x18 * x499;
	const FLT x501 = x25 * x496;
	const FLT x502 = x31 * x494;
	const FLT x503 = x18 * x502;
	const FLT x504 = (-1 * x21 * x503) + (x21 * x501) + (x496 * x489) + (x496 * x497) + (-1 * x492 * x489) + (x21 * x500) + (-1 * x21 * x498) + (-1 * x497 * x492);
	const FLT x505 = 1. / (x38 * sqrt(x38));
	const FLT x506 = x33 * x505;
	const FLT x507 = x504 * x506;
	const FLT x508 = x24 * x507;
	const FLT x509 = 1.0/2.0 * x12;
	const FLT x510 = x6 * x509;
	const FLT x511 = x42 * x17;
	const FLT x512 = 0.5 * x31;
	const FLT x513 = x18 * x512;
	const FLT x514 = x21 * x513;
	const FLT x515 = x44 * x505;
	const FLT x516 = 1.0/2.0 * x515;
	const FLT x517 = x516 * x504;
	const FLT x518 = x493 * x480;
	const FLT x519 = x24 * x518;
	const FLT x520 = x21 * x519;
	const FLT x521 = x50 * x520;
	const FLT x522 = x21 * x507;
	const FLT x523 = x47 * x17;
	const FLT x524 = x6 * x523;
	const FLT x525 = x21 * x518;
	const FLT x526 = x524 * x525;
	const FLT x527 = x22 * x518;
	const FLT x528 = x40 * x17;
	const FLT x529 = (-1 * x528 * x527) + x526 + x41 + (-1 * x522 * x356) + (x53 * x487) + (-1 * x484) + (x510 * x507) + (-1 * x488 * x508) + x486 + (-1 * x15 * x517) + (-1 * x514 * x511) + (-1 * x521);
	const FLT x530 = 1.0/2.0 * x15;
	const FLT x531 = x6 * x507;
	const FLT x532 = x21 * x509;
	const FLT x533 = (x511 * x520) + (-1 * x55 * x485);
	const FLT x534 = x6 * x525;
	const FLT x535 = (-1 * x528 * x534) + (x53 * x483);
	const FLT x536 = x535 + x533 + (-1 * x523 * x527) + (-1 * x50 * x514) + (x58 * x487) + (-1 * x531 * x356) + (-1 * x10 * x517) + x48 + (-1 * x507 * x532) + (x508 * x530);
	const FLT x537 = -1 * x43;
	const FLT x538 = x523 * x520;
	const FLT x539 = x24 * x509;
	const FLT x540 = x58 * x485;
	const FLT x541 = x21 * x530;
	const FLT x542 = x515 * x356;
	const FLT x543 = (x50 * x534) + (-1 * x46 * x483);
	const FLT x544 = x543 + (-1 * x504 * x542) + (-1 * x540) + x538 + (x507 * x541) + (-1 * x55 * x487) + (x511 * x527) + x537 + (-1 * x514 * x528) + (x507 * x539) + (x488 * x531);
	const FLT x545 = -1 * x51;
	const FLT x546 = x511 * x534;
	const FLT x547 = x55 * x483;
	const FLT x548 = (-1 * x520 * x528) + (x53 * x485);
	const FLT x549 = (x488 * x522) + x547 + (x50 * x527) + (-1 * x546) + x548 + x545 + (-1 * x46 * x487) + (-1 * x12 * x517) + (-1 * x514 * x523) + (-1 * x531 * x530) + (-1 * x508 * x356);
	const FLT x550 = (x549 * sensor_pt[2]) + (-1 * x536 * sensor_pt[1]) + (x544 * sensor_pt[0]);
	const FLT x551 = (x529 * sensor_pt[1]) + (-1 * x549 * sensor_pt[0]) + (x544 * sensor_pt[2]);
	const FLT x552 = (x550 * x393) + (-1 * x551 * x396) + (-1 * x529 * x372) + (x536 * x394);
	const FLT x553 = (x536 * sensor_pt[0]) + (-1 * x529 * sensor_pt[2]) + (x544 * sensor_pt[1]);
	const FLT x554 = (x549 * x372) + (-1 * x553 * x393) + (-1 * x536 * x398) + (x551 * x399);
	const FLT x555 = (x553 * x396) + (x529 * x398) + (-1 * x549 * x394) + (-1 * x550 * x399);
	const FLT x556 = (-1 * x82 * x555) + (x84 * x552) + (x79 * x554);
	const FLT x557 = (-1 * x84 * x554) + (x555 * x119) + (x79 * x552);
	const FLT x558 = (x556 * x118) + x555 + (-1 * x557 * x153);
	const FLT x559 = (-1 * x552 * x119) + (x82 * x554) + (x79 * x555);
	const FLT x560 = x554 + (x557 * x176) + (-1 * x559 * x118);
	const FLT x561 = (x559 * x153) + x552 + (-1 * x556 * x176);
	const FLT x562 = (-1 * x194 * ((x561 * x205) + (-1 * ((x558 * x196) + (x560 * x195)) * x197))) + (-1 * ((-1 * x560 * x187) + (x558 * x168)) * x190);
	const FLT x563 = x25 * x481;
	const FLT x564 = x25 * x518;
	const FLT x565 = x24 * x492;
	const FLT x566 = x24 * x496;
	const FLT x567 = x24 * x18;
	const FLT x568 = (x24 * x24 * x24) * x490;
	const FLT x569 = (x499 * x567) + (x495 * x568) + (-1 * x491 * x568) + (x22 * x566) + (-1 * x22 * x565) + (-1 * x502 * x567) + (x19 * x566) + (-1 * x19 * x565);
	const FLT x570 = x506 * x569;
	const FLT x571 = x570 * x356;
	const FLT x572 = x24 * x570;
	const FLT x573 = x515 * x569;
	const FLT x574 = 1.0/2.0 * x573;
	const FLT x575 = x512 * x567;
	const FLT x576 = x6 * x519;
	const FLT x577 = x6 * x24 * x481;
	const FLT x578 = (x53 * x577) + (-1 * x528 * x576);
	const FLT x579 = x578 + x537 + (-1 * x55 * x563) + (x511 * x564) + (-1 * x532 * x570) + (-1 * x50 * x575) + (-1 * x6 * x571) + (-1 * x10 * x574) + (-1 * x538) + (x530 * x572) + x540;
	const FLT x580 = x511 * x576;
	const FLT x581 = x6 * x570;
	const FLT x582 = x55 * x577;
	const FLT x583 = (x21 * x488 * x570) + (x53 * x563) + (-1 * x486) + (-1 * x12 * x574) + (-1 * x528 * x564) + (-1 * x580) + x521 + (-1 * x24 * x571) + x582 + x41 + (-1 * x530 * x581) + (-1 * x523 * x575);
	const FLT x584 = x50 * x576;
	const FLT x585 = x46 * x577;
	const FLT x586 = -1 * x48;
	const FLT x587 = (-1 * x528 * x575) + (-1 * x58 * x563) + (x509 * x572) + x586 + (x488 * x581) + (x541 * x570) + x584 + x533 + (-1 * x573 * x356) + (x523 * x564) + (-1 * x585);
	const FLT x588 = (x519 * x524) + (-1 * x58 * x577);
	const FLT x589 = x548 + (-1 * x15 * x574) + (-1 * x488 * x572) + (-1 * x511 * x575) + x51 + (-1 * x50 * x564) + x588 + (x46 * x563) + (x510 * x570) + (-1 * x21 * x571);
	const FLT x590 = (x589 * sensor_pt[1]) + (-1 * x583 * sensor_pt[0]) + (x587 * sensor_pt[2]);
	const FLT x591 = (x579 * sensor_pt[0]) + (-1 * x589 * sensor_pt[2]) + (x587 * sensor_pt[1]);
	const FLT x592 = (-1 * x591 * x393) + (x583 * x372) + (-1 * x579 * x398) + (x590 * x399);
	const FLT x593 = (x583 * sensor_pt[2]) + (-1 * x579 * sensor_pt[1]) + (x587 * sensor_pt[0]);
	const FLT x594 = (-1 * x590 * x396) + (-1 * x589 * x372) + (x593 * x393) + (x579 * x394);
	const FLT x595 = (-1 * x593 * x399) + (x591 * x396) + (-1 * x583 * x394) + (x589 * x398);
	const FLT x596 = x78 * x595;
	const FLT x597 = (-1 * x3 * x596) + (x79 * x592) + (x84 * x594);
	const FLT x598 = (-1 * x84 * x592) + (x68 * x596) + (x79 * x594);
	const FLT x599 = x595 + (x597 * x118) + (-1 * x598 * x153);
	const FLT x600 = (-1 * x594 * x119) + (x82 * x592) + (x63 * x596);
	const FLT x601 = x592 + (x598 * x176) + (-1 * x600 * x118);
	const FLT x602 = x594 + (-1 * x597 * x176) + (x600 * x153);
	const FLT x603 = (-1 * x194 * ((x602 * x205) + (-1 * ((x599 * x196) + (x601 * x195)) * x197))) + (-1 * ((-1 * x601 * x187) + (x599 * x168)) * x190);
	const FLT x604 = x19 * x518;
	const FLT x605 = x6 * x513;
	const FLT x606 = x19 * x481;
	const FLT x607 = x6 * x22;
	const FLT x608 = x6 * x6 * x6;
	const FLT x609 = (x496 * x608) + (-1 * x492 * x607) + (-1 * x492 * x608) + (x496 * x607) + (-1 * x6 * x503) + (x6 * x500) + (x6 * x501) + (-1 * x6 * x498);
	const FLT x610 = x609 * x506;
	const FLT x611 = x21 * x610;
	const FLT x612 = x609 * x516;
	const FLT x613 = x6 * x610;
	const FLT x614 = x24 * x610;
	const FLT x615 = x535 + (x613 * x509) + (-1 * x15 * x612) + (-1 * x611 * x356) + (-1 * x488 * x614) + (-1 * x584) + x585 + (x604 * x523) + (-1 * x58 * x606) + x586 + (-1 * x605 * x511);
	const FLT x616 = (-1 * x611 * x509) + x41 + (-1 * x50 * x605) + (-1 * x613 * x356) + x484 + x580 + (x614 * x530) + (-1 * x526) + (x53 * x606) + (-1 * x604 * x528) + (-1 * x582) + (-1 * x10 * x612);
	const FLT x617 = (-1 * x609 * x542) + (-1 * x605 * x528) + (x610 * x539) + x588 + (x50 * x604) + (-1 * x46 * x606) + x545 + (-1 * x547) + (x488 * x613) + (x610 * x541) + x546;
	const FLT x618 = x578 + (-1 * x12 * x612) + (-1 * x604 * x511) + (-1 * x513 * x524) + x43 + x543 + (x55 * x606) + (-1 * x613 * x530) + (x488 * x611) + (-1 * x614 * x356);
	const FLT x619 = (x617 * sensor_pt[2]) + (x615 * sensor_pt[1]) + (-1 * x618 * sensor_pt[0]);
	const FLT x620 = (x618 * sensor_pt[2]) + (-1 * x616 * sensor_pt[1]) + (x617 * sensor_pt[0]);
	const FLT x621 = (x620 * x393) + (-1 * x619 * x396) + (-1 * x615 * x372) + (x616 * x394);
	const FLT x622 = (x616 * sensor_pt[0]) + (-1 * x615 * sensor_pt[2]) + (x617 * sensor_pt[1]);
	const FLT x623 = (x618 * x372) + (-1 * x616 * x398) + (x619 * x399) + (-1 * x622 * x393);
	const FLT x624 = (-1 * x620 * x399) + (x622 * x396) + (-1 * x618 * x394) + (x615 * x398);
	const FLT x625 = (-1 * x82 * x624) + (x84 * x621) + (x79 * x623);
	const FLT x626 = (-1 * x84 * x623) + (x624 * x119) + (x79 * x621);
	const FLT x627 = (x625 * x118) + x624 + (-1 * x626 * x153);
	const FLT x628 = (x82 * x623) + (-1 * x621 * x119) + (x79 * x624);
	const FLT x629 = (x626 * x176) + x623 + (-1 * x628 * x118);
	const FLT x630 = x621 + (x628 * x153) + (-1 * x625 * x176);
	const FLT x631 = (-1 * x194 * ((x630 * x205) + (-1 * ((x627 * x196) + (x629 * x195)) * x197))) + (-1 * ((-1 * x629 * x187) + (x627 * x168)) * x190);
	const FLT x632 = dt * x295;
	const FLT x633 = dt * x292;
	const FLT x634 = x633 + x632;
	const FLT x635 = -1 * dt * x298;
	const FLT x636 = (-1 * dt * x299) + dt;
	const FLT x637 = x636 + x635;
	const FLT x638 = dt * x303;
	const FLT x639 = dt * x301;
	const FLT x640 = (-1 * x639) + x638;
	const FLT x641 = (-1 * x194 * ((x640 * x205) + (-1 * ((x634 * x196) + (x637 * x195)) * x197))) + (-1 * ((-1 * x637 * x187) + (x634 * x168)) * x190);
	const FLT x642 = dt * x308;
	const FLT x643 = dt * x307;
	const FLT x644 = (-1 * x643) + x642;
	const FLT x645 = x638 + x639;
	const FLT x646 = -1 * dt * x313;
	const FLT x647 = x636 + x646;
	const FLT x648 = (-1 * x194 * ((x647 * x205) + (-1 * ((x644 * x196) + (x645 * x195)) * x197))) + (-1 * ((-1 * x645 * x187) + (x644 * x168)) * x190);
	const FLT x649 = x646 + x635 + dt;
	const FLT x650 = (-1 * x632) + x633;
	const FLT x651 = x642 + x643;
	const FLT x652 = (-1 * x194 * ((x651 * x205) + (-1 * ((x649 * x196) + (x650 * x195)) * x197))) + (-1 * ((-1 * x650 * x187) + (x649 * x168)) * x190);
	const FLT x653 = x204 * x191 * x194;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), (x209 * x206) + x206 + (((-1 * x203 * x187) + (x210 * x161)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), x255 + (x209 * x255) + (((-1 * x254 * x187) + (x210 * x242)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), x287 + (x209 * x287) + (((-1 * x286 * x187) + (x210 * x282)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), x306 + (x209 * x306) + (((-1 * x305 * x187) + (x210 * x297)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), x316 + (x209 * x316) + (((-1 * x315 * x187) + (x210 * x310)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), x321 + (x209 * x321) + (((-1 * x320 * x187) + (x210 * x317)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), x335 + (x209 * x335) + (((-1 * x334 * x187) + (x210 * x327)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), x342 + (x209 * x342) + (((-1 * x341 * x187) + (x210 * x338)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), x346 + (x209 * x346) + (((-1 * x345 * x187) + (x210 * x343)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), x409 + (x409 * x209) + (((-1 * x408 * x187) + (x405 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), x438 + (x438 * x209) + (((-1 * x437 * x187) + (x434 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), x464 + (x464 * x209) + (((-1 * x463 * x187) + (x460 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), x470 + (x470 * x209) + (((-1 * x469 * x187) + (x465 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), x475 + (x475 * x209) + (((-1 * x474 * x187) + (x471 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), x479 + (x479 * x209) + (((-1 * x478 * x187) + (x476 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x562 + (x562 * x209) + (((-1 * x561 * x187) + (x558 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), (x603 * x209) + x603 + (((-1 * x602 * x187) + (x599 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x631 + (x631 * x209) + (((-1 * x630 * x187) + (x627 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), (x641 * x209) + x641 + (((-1 * x640 * x187) + (x634 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), x648 + (x648 * x209) + (((-1 * x647 * x187) + (x644 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), x652 + (x652 * x209) + (((-1 * x651 * x187) + (x649 * x210)) * x212));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.curve)/sizeof(FLT), x211 * x211);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), -1 * cos(x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), x209);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.phase)/sizeof(FLT), -1 + (-1 * x209));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.tilt)/sizeof(FLT), (-1 * x653 * x209) + (-1 * x653));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]

static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_error_model(Hx, dt, _x0, error_model, sensor_pt);
    }
}
// Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]]
static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x2 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x3 = (-1 * x2 * (*_x0).Object.Pose.Rot[1]) + (x1 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x0 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2];
	const FLT x4 = dt * dt;
	const FLT x5 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x6 = x4 * (x5 * x5);
	const FLT x7 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x8 = x4 * (x7 * x7);
	const FLT x9 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x10 = x4 * (x9 * x9);
	const FLT x11 = 1e-10 + x10 + x6 + x8;
	const FLT x12 = sqrt(x11);
	const FLT x13 = 0.5 * x12;
	const FLT x14 = cos(x13);
	const FLT x15 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x16 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x17 = (x16 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x15 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x2 * (*_x0).Object.Pose.Rot[0]);
	const FLT x18 = (*_x0).Object.Pose.Rot[0] + (-1 * x16 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x2 * (*_x0).Object.Pose.Rot[3]) + (-1 * x15 * (*error_model).Object.Pose.AxisAngleRot[1]);
	const FLT x19 = (*_x0).Object.Pose.Rot[1] + (x1 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x2 * (*_x0).Object.Pose.Rot[2]);
	const FLT x20 = sin(x13);
	const FLT x21 = (x20 * x20) * (1. / x11);
	const FLT x22 = (1. / sqrt((x19 * x19) + (x18 * x18) + (x17 * x17) + (x3 * x3))) * (1. / sqrt((x8 * x21) + (x14 * x14) + (x6 * x21) + (x21 * x10)));
	const FLT x23 = x22 * x14;
	const FLT x24 = x3 * x23;
	const FLT x25 = dt * x22 * x20 * (1. / x12);
	const FLT x26 = x9 * x25;
	const FLT x27 = x26 * x18;
	const FLT x28 = x7 * x25;
	const FLT x29 = x28 * x17;
	const FLT x30 = x5 * x25;
	const FLT x31 = x30 * x19;
	const FLT x32 = (-1 * x31) + x29 + (-1 * x24) + (-1 * x27);
	const FLT x33 = x3 * x30;
	const FLT x34 = x26 * x17;
	const FLT x35 = x23 * x19;
	const FLT x36 = x28 * x18;
	const FLT x37 = x36 + x35 + (-1 * x33) + x34;
	const FLT x38 = 2 * x37;
	const FLT x39 = (-1 * x28 * x19) + (x23 * x18) + (-1 * x30 * x17) + (-1 * x3 * x26);
	const FLT x40 = x30 * x18;
	const FLT x41 = x26 * x19;
	const FLT x42 = x23 * x17;
	const FLT x43 = x3 * x28;
	const FLT x44 = x43 + x42 + x40 + (-1 * x41);
	const FLT x45 = 2 * x44;
	const FLT x46 = x45 * x39;
	const FLT x47 = x46 + (-1 * x32 * x38);
	const FLT x48 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x49 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x50 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x51 = (x50 * (*_x0).Lighthouse.Rot[1]) + (x49 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x48 * (*_x0).Lighthouse.Rot[2]);
	const FLT x52 = x51 * x51;
	const FLT x53 = (-1 * x48 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x49 * (*_x0).Lighthouse.Rot[3]) + (-1 * x50 * (*_x0).Lighthouse.Rot[2]);
	const FLT x54 = x53 * x53;
	const FLT x55 = 0.5 * (*_x0).Lighthouse.Rot[3];
	const FLT x56 = (x50 * (*_x0).Lighthouse.Rot[0]) + (x55 * (*error_model).Lighthouse.AxisAngleRot[0]) + (-1 * x49 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[2];
	const FLT x57 = x56 * x56;
	const FLT x58 = (*_x0).Lighthouse.Rot[1] + (-1 * x55 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x48 * (*_x0).Lighthouse.Rot[0]) + (x49 * (*_x0).Lighthouse.Rot[2]);
	const FLT x59 = x58 * x58;
	const FLT x60 = x57 + x59 + x52 + x54;
	const FLT x61 = 1. / x60;
	const FLT x62 = (1. / sqrt((x61 * x59) + (x61 * x52) + (x61 * x54) + (x61 * x57))) * (1. / sqrt(x60));
	const FLT x63 = x62 * x51;
	const FLT x64 = (-1 * x29) + x31 + x27 + x24;
	const FLT x65 = 2 * x64;
	const FLT x66 = 1 + (x65 * x32) + (-2 * (x44 * x44));
	const FLT x67 = x62 * x53;
	const FLT x68 = x65 * x39;
	const FLT x69 = (x44 * x38) + (-1 * x68);
	const FLT x70 = x62 * x56;
	const FLT x71 = (-1 * x70 * x69) + (x63 * x47) + (x67 * x66);
	const FLT x72 = 2 * x70;
	const FLT x73 = x62 * x58;
	const FLT x74 = (-1 * x63 * x66) + (x73 * x69) + (x67 * x47);
	const FLT x75 = 2 * x73;
	const FLT x76 = x69 + (x71 * x72) + (-1 * x75 * x74);
	const FLT x77 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x78 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x79 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x80 = (x73 * x79) + (x77 * x67) + (-1 * x78 * x63);
	const FLT x81 = (x70 * x78) + (x79 * x67) + (-1 * x73 * x77);
	const FLT x82 = 1.0/2.0 * dt * fabs(dt);
	const FLT x83 = (-1 * x64 * sensor_pt[0]) + (x39 * sensor_pt[2]) + (x37 * sensor_pt[1]);
	const FLT x84 = (-1 * x44 * sensor_pt[1]) + (x39 * sensor_pt[0]) + (x64 * sensor_pt[2]);
	const FLT x85 = (*_x0).Object.Pose.Pos[1] + (x82 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x84 * x44) + (-1 * x83 * x37)));
	const FLT x86 = (-1 * x37 * sensor_pt[2]) + (x39 * sensor_pt[1]) + (x44 * sensor_pt[0]);
	const FLT x87 = (*_x0).Object.Pose.Pos[0] + (x82 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (2 * ((x83 * x64) + (-1 * x86 * x44))) + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0]));
	const FLT x88 = (*_x0).Object.Pose.Pos[2] + (2 * ((x86 * x37) + (-1 * x84 * x64))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x82 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x89 = (x88 * x73) + (x85 * x67) + (-1 * x87 * x63);
	const FLT x90 = (x87 * x70) + (x88 * x67) + (-1 * x85 * x73);
	const FLT x91 = (-1 * (x78 + (2 * ((-1 * x81 * x70) + (x80 * x63))))) + x87 + (2 * ((-1 * x70 * x90) + (x89 * x63)));
	const FLT x92 = (x77 * x63) + (-1 * x70 * x79) + (x78 * x67);
	const FLT x93 = (x85 * x63) + (x87 * x67) + (-1 * x88 * x70);
	const FLT x94 = x88 + (-1 * (x79 + (2 * ((-1 * x80 * x73) + (x70 * x92))))) + (2 * ((-1 * x89 * x73) + (x70 * x93)));
	const FLT x95 = x94 * x94;
	const FLT x96 = 1. / x95;
	const FLT x97 = x91 * x96;
	const FLT x98 = 1. / x94;
	const FLT x99 = (-1 * x73 * x47) + (x70 * x66) + (x67 * x69);
	const FLT x100 = 2 * x63;
	const FLT x101 = x66 + (-1 * x72 * x99) + (x74 * x100);
	const FLT x102 = (x91 * x91) + x95;
	const FLT x103 = 1. / x102;
	const FLT x104 = x95 * x103;
	const FLT x105 = 2 * x91;
	const FLT x106 = 2 * x94;
	const FLT x107 = x85 + (2 * ((-1 * x63 * x93) + (x73 * x90))) + (-1 * (x77 + (2 * ((-1 * x63 * x92) + (x81 * x73)))));
	const FLT x108 = (*error_model).BSD0.tilt + (*_x0).BSD0.tilt;
	const FLT x109 = 1.0/2.0 * (1. / (x102 * sqrt(x102))) * x108 * x107;
	const FLT x110 = x47 + (-1 * x71 * x100) + (x75 * x99);
	const FLT x111 = (1. / sqrt(x102)) * x108;
	const FLT x112 = x107 * x107;
	const FLT x113 = 1. / sqrt(1 + (-1 * x103 * (x108 * x108) * x112));
	const FLT x114 = (-1 * x113 * ((x110 * x111) + (-1 * x109 * ((x76 * x106) + (x101 * x105))))) + (-1 * x104 * ((-1 * x98 * x101) + (x76 * x97)));
	const FLT x115 = -1 * x94;
	const FLT x116 = sin(1.5707963267949 + (*_x0).BSD0.gibpha + (*error_model).BSD0.gibpha + (-1 * ((*error_model).BSD0.phase + (*_x0).BSD0.phase)) + (-1 * atan2(x91, x115)) + (-1 * asin(x107 * x111))) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag);
	const FLT x117 = x96 * x107;
	const FLT x118 = 2 * x95 * (1. / (x95 + x112)) * atan2(x107, x115) * ((*error_model).BSD0.curve + (*_x0).BSD0.curve);
	const FLT x119 = (-1 * x40) + (-1 * x43) + x41 + (-1 * x42);
	const FLT x120 = 1 + (x45 * x119) + (-2 * (x37 * x37));
	const FLT x121 = (x65 * x37) + (-1 * x46);
	const FLT x122 = x38 * x39;
	const FLT x123 = x122 + (-1 * x65 * x119);
	const FLT x124 = (-1 * x70 * x123) + (x63 * x120) + (x67 * x121);
	const FLT x125 = (-1 * x63 * x121) + (x73 * x123) + (x67 * x120);
	const FLT x126 = (x72 * x124) + x123 + (-1 * x75 * x125);
	const FLT x127 = (-1 * x73 * x120) + (x70 * x121) + (x67 * x123);
	const FLT x128 = (-1 * x72 * x127) + x121 + (x100 * x125);
	const FLT x129 = x120 + (-1 * x100 * x124) + (x75 * x127);
	const FLT x130 = (-1 * x113 * ((x111 * x129) + (-1 * ((x106 * x126) + (x105 * x128)) * x109))) + (-1 * ((-1 * x98 * x128) + (x97 * x126)) * x104);
	const FLT x131 = (x65 * x44) + (-1 * x122);
	const FLT x132 = (-1 * x35) + (-1 * x36) + (-1 * x34) + x33;
	const FLT x133 = x68 + (-1 * x45 * x132);
	const FLT x134 = 1 + (x38 * x132) + (-2 * (x64 * x64));
	const FLT x135 = (-1 * x70 * x134) + (x63 * x131) + (x67 * x133);
	const FLT x136 = (-1 * x63 * x133) + (x73 * x134) + (x67 * x131);
	const FLT x137 = (x72 * x135) + x134 + (-1 * x75 * x136);
	const FLT x138 = x96 * x137;
	const FLT x139 = (-1 * x73 * x131) + (x70 * x133) + (x67 * x134);
	const FLT x140 = x133 + (-1 * x72 * x139) + (x100 * x136);
	const FLT x141 = x131 + (-1 * x100 * x135) + (x75 * x139);
	const FLT x142 = (-1 * x113 * ((x111 * x141) + (-1 * ((x106 * x137) + (x105 * x140)) * x109))) + (-1 * ((-1 * x98 * x140) + (x91 * x138)) * x104);
	cnMatrixOptionalSet(Hx, 0, 0, x114 + (x114 * x116) + (((-1 * x98 * x110) + (x76 * x117)) * x118));
	cnMatrixOptionalSet(Hx, 0, 1, x130 + (x116 * x130) + (x118 * ((-1 * x98 * x129) + (x117 * x126))));
	cnMatrixOptionalSet(Hx, 0, 2, (x116 * x142) + x142 + (x118 * ((-1 * x98 * x141) + (x107 * x138))));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]]

static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen1(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_x_gen1_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt);
    }
}
static inline FLT SurviveJointKalmanErrorModel_LightMeas_y_gen1(const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x3 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x4 = (*_x0).Lighthouse.Rot[0] + (-1 * x3 * (*_x0).Lighthouse.Rot[1]) + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x5 = (x2 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x3 * (*_x0).Lighthouse.Rot[2]);
	const FLT x6 = x5 * x5;
	const FLT x7 = x4 * x4;
	const FLT x8 = (-1 * x1 * (*_x0).Lighthouse.Rot[1]) + (x3 * (*_x0).Lighthouse.Rot[3]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2];
	const FLT x9 = x8 * x8;
	const FLT x10 = (*_x0).Lighthouse.Rot[1] + (x3 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (x1 * (*_x0).Lighthouse.Rot[2]);
	const FLT x11 = x10 * x10;
	const FLT x12 = x11 + x9 + x6 + x7;
	const FLT x13 = 1. / x12;
	const FLT x14 = (1. / sqrt(x12)) * (1. / sqrt((x13 * x11) + (x7 * x13) + (x6 * x13) + (x9 * x13)));
	const FLT x15 = x4 * x14;
	const FLT x16 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x17 = x5 * x14;
	const FLT x18 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x19 = x14 * x10;
	const FLT x20 = (x0 * x15) + (x19 * x18) + (-1 * x17 * x16);
	const FLT x21 = x8 * x14;
	const FLT x22 = (x21 * x16) + (x15 * x18) + (-1 * x0 * x19);
	const FLT x23 = 1.0/2.0 * dt * fabs(dt);
	const FLT x24 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x25 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x26 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x27 = (x25 * (*_x0).Object.Pose.Rot[0]) + (x24 * (*_x0).Object.Pose.Rot[3]) + (-1 * x26 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[2];
	const FLT x28 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x29 = dt * dt;
	const FLT x30 = (x28 * x28) * x29;
	const FLT x31 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x32 = (x31 * x31) * x29;
	const FLT x33 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x34 = (x33 * x33) * x29;
	const FLT x35 = 1e-10 + x34 + x30 + x32;
	const FLT x36 = sqrt(x35);
	const FLT x37 = 0.5 * x36;
	const FLT x38 = sin(x37);
	const FLT x39 = (x25 * (*_x0).Object.Pose.Rot[1]) + (-1 * x24 * (*_x0).Object.Pose.Rot[2]) + (*_x0).Object.Pose.Rot[3] + (x26 * (*_x0).Object.Pose.Rot[0]);
	const FLT x40 = (*_x0).Object.Pose.Rot[0] + (-1 * x26 * (*_x0).Object.Pose.Rot[3]) + (-1 * x24 * (*_x0).Object.Pose.Rot[1]) + (-1 * x25 * (*_x0).Object.Pose.Rot[2]);
	const FLT x41 = (*_x0).Object.Pose.Rot[1] + (x24 * (*_x0).Object.Pose.Rot[0]) + (-1 * x25 * (*_x0).Object.Pose.Rot[3]) + (x26 * (*_x0).Object.Pose.Rot[2]);
	const FLT x42 = (1. / x35) * (x38 * x38);
	const FLT x43 = cos(x37);
	const FLT x44 = (1. / sqrt((x40 * x40) + (x39 * x39) + (x41 * x41) + (x27 * x27))) * (1. / sqrt((x43 * x43) + (x42 * x30) + (x42 * x32) + (x42 * x34)));
	const FLT x45 = dt * x44 * (1. / x36) * x38;
	const FLT x46 = x45 * x28;
	const FLT x47 = x45 * x33;
	const FLT x48 = x43 * x44;
	const FLT x49 = x45 * x31;
	const FLT x50 = (x41 * x48) + (-1 * x46 * x27) + (x40 * x49) + (x47 * x39);
	const FLT x51 = (x40 * x48) + (-1 * x41 * x49) + (-1 * x46 * x39) + (-1 * x47 * x27);
	const FLT x52 = (-1 * x49 * x39) + (x41 * x46) + (x40 * x47) + (x48 * x27);
	const FLT x53 = (x51 * sensor_pt[2]) + (-1 * x52 * sensor_pt[0]) + (x50 * sensor_pt[1]);
	const FLT x54 = (x49 * x27) + (x48 * x39) + (x40 * x46) + (-1 * x41 * x47);
	const FLT x55 = (-1 * x54 * sensor_pt[1]) + (x51 * sensor_pt[0]) + (x52 * sensor_pt[2]);
	const FLT x56 = (*_x0).Object.Pose.Pos[1] + (x23 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x54 * x55) + (-1 * x50 * x53)));
	const FLT x57 = (x51 * sensor_pt[1]) + (-1 * x50 * sensor_pt[2]) + (x54 * sensor_pt[0]);
	const FLT x58 = (*_x0).Object.Pose.Pos[0] + (x23 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (2 * ((x53 * x52) + (-1 * x54 * x57))) + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0]));
	const FLT x59 = (*_x0).Object.Pose.Pos[2] + (2 * ((x50 * x57) + (-1 * x52 * x55))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x23 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x60 = (x59 * x19) + (x56 * x15) + (-1 * x58 * x17);
	const FLT x61 = (x58 * x21) + (x59 * x15) + (-1 * x56 * x19);
	const FLT x62 = x58 + (-1 * (x16 + (2 * ((-1 * x22 * x21) + (x20 * x17))))) + (2 * ((-1 * x61 * x21) + (x60 * x17)));
	const FLT x63 = (x0 * x17) + (-1 * x21 * x18) + (x15 * x16);
	const FLT x64 = (x56 * x17) + (x58 * x15) + (-1 * x59 * x21);
	const FLT x65 = x59 + (-1 * (x18 + (2 * ((-1 * x20 * x19) + (x63 * x21))))) + (2 * ((-1 * x60 * x19) + (x64 * x21)));
	const FLT x66 = -1 * x65;
	const FLT x67 = x56 + (2 * ((-1 * x64 * x17) + (x61 * x19))) + (-1 * (x0 + (2 * ((-1 * x63 * x17) + (x22 * x19)))));
	const FLT x68 = (-1 * ((*error_model).BSD1.phase + (*_x0).BSD1.phase)) + (-1 * atan2(-1 * x67, x66)) + (-1 * asin((1. / sqrt((x67 * x67) + (x65 * x65))) * x62 * ((*error_model).BSD1.tilt + (*_x0).BSD1.tilt)));
	return x68 + ((atan2(x62, x66) * atan2(x62, x66)) * ((*error_model).BSD1.curve + (*_x0).BSD1.curve)) + (-1 * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag) * cos(1.5707963267949 + x68 + (*error_model).BSD1.gibpha + (*_x0).BSD1.gibpha));
}

// Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_x0(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x1 = 0.5 * (*_x0).Lighthouse.Rot[0];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x3 = (x2 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x1 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x0 * (*_x0).Lighthouse.Rot[2]);
	const FLT x4 = x3 * x3;
	const FLT x5 = 0.5 * (*_x0).Lighthouse.Rot[3];
	const FLT x6 = (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x5 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x7 = x6 * x6;
	const FLT x8 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x9 = (-1 * x8 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x5 * (*error_model).Lighthouse.AxisAngleRot[0]) + (*_x0).Lighthouse.Rot[2];
	const FLT x10 = x9 * x9;
	const FLT x11 = (*_x0).Lighthouse.Rot[1] + (x1 * (*error_model).Lighthouse.AxisAngleRot[0]) + (-1 * x5 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x8 * (*_x0).Lighthouse.Rot[2]);
	const FLT x12 = x11 * x11;
	const FLT x13 = x10 + x12 + x4 + x7;
	const FLT x14 = 1. / x13;
	const FLT x15 = x4 * x14;
	const FLT x16 = x14 * x10;
	const FLT x17 = x14 * x12;
	const FLT x18 = x17 + (x7 * x14) + x15 + x16;
	const FLT x19 = 1. / x18;
	const FLT x20 = 2 * x11;
	const FLT x21 = x20 * x14;
	const FLT x22 = x21 * x19;
	const FLT x23 = x3 * x22;
	const FLT x24 = -1 * x23;
	const FLT x25 = 2 * x6;
	const FLT x26 = x25 * x14;
	const FLT x27 = x9 * x19;
	const FLT x28 = x26 * x27;
	const FLT x29 = -1 * x28;
	const FLT x30 = x29 + x24;
	const FLT x31 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x32 = 1. / sqrt(x18);
	const FLT x33 = 1. / sqrt(x13);
	const FLT x34 = x32 * x33;
	const FLT x35 = x31 * x34;
	const FLT x36 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x37 = x34 * x36;
	const FLT x38 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x39 = x34 * x38;
	const FLT x40 = (x39 * x11) + (x6 * x35) + (-1 * x3 * x37);
	const FLT x41 = x3 * x34;
	const FLT x42 = (x9 * x37) + (x6 * x39) + (-1 * x35 * x11);
	const FLT x43 = x9 * x34;
	const FLT x44 = dt * fabs(dt);
	const FLT x45 = 1.0/2.0 * x44;
	const FLT x46 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x47 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x48 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x49 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x50 = (x47 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x49 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x48 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2];
	const FLT x51 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x52 = (x49 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x51 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x48 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x53 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x54 = (-1 * x53 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[0] + (-1 * x47 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x51 * (*error_model).Object.Pose.AxisAngleRot[1]);
	const FLT x55 = (*_x0).Object.Pose.Rot[1] + (x53 * (*_x0).Object.Pose.Rot[0]) + (-1 * x47 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x51 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x56 = (x55 * x55) + (x54 * x54) + (x52 * x52) + (x50 * x50);
	const FLT x57 = 1. / sqrt(x56);
	const FLT x58 = dt * dt;
	const FLT x59 = x46 * x46;
	const FLT x60 = x58 * x59;
	const FLT x61 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x62 = x61 * x61;
	const FLT x63 = x62 * x58;
	const FLT x64 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x65 = x64 * x64;
	const FLT x66 = x65 * x58;
	const FLT x67 = 1e-10 + x66 + x60 + x63;
	const FLT x68 = sqrt(x67);
	const FLT x69 = 0.5 * x68;
	const FLT x70 = sin(x69);
	const FLT x71 = x70 * x70;
	const FLT x72 = 1. / x67;
	const FLT x73 = x71 * x72;
	const FLT x74 = cos(x69);
	const FLT x75 = (x73 * x63) + (x73 * x60) + (x74 * x74) + (x73 * x66);
	const FLT x76 = 1. / sqrt(x75);
	const FLT x77 = x76 * x57;
	const FLT x78 = x70 * (1. / x68);
	const FLT x79 = dt * x78;
	const FLT x80 = x79 * x77;
	const FLT x81 = x80 * x50;
	const FLT x82 = x80 * x52;
	const FLT x83 = x74 * x77;
	const FLT x84 = x83 * x55;
	const FLT x85 = x80 * x54;
	const FLT x86 = x84 + (-1 * x81 * x46) + (x85 * x61) + (x82 * x64);
	const FLT x87 = x83 * x54;
	const FLT x88 = x77 * x55;
	const FLT x89 = x88 * x79;
	const FLT x90 = (-1 * x89 * x61) + x87 + (-1 * x82 * x46) + (-1 * x81 * x64);
	const FLT x91 = x83 * x50;
	const FLT x92 = (x89 * x46) + (x85 * x64) + (-1 * x82 * x61) + x91;
	const FLT x93 = (-1 * x92 * sensor_pt[0]) + (x90 * sensor_pt[2]) + (x86 * sensor_pt[1]);
	const FLT x94 = x83 * x52;
	const FLT x95 = (x81 * x61) + x94 + (x85 * x46) + (-1 * x89 * x64);
	const FLT x96 = (x90 * sensor_pt[0]) + (-1 * x95 * sensor_pt[1]) + (x92 * sensor_pt[2]);
	const FLT x97 = (*_x0).Object.Pose.Pos[1] + (x45 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x96 * x95) + (-1 * x86 * x93)));
	const FLT x98 = x97 * x34;
	const FLT x99 = (-1 * x86 * sensor_pt[2]) + (x90 * sensor_pt[1]) + (x95 * sensor_pt[0]);
	const FLT x100 = (*_x0).Object.Pose.Pos[0] + (x45 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0] + (2 * ((x93 * x92) + (-1 * x99 * x95)));
	const FLT x101 = x34 * x100;
	const FLT x102 = (*_x0).Object.Pose.Pos[2] + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (2 * ((x86 * x99) + (-1 * x92 * x96))) + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x45 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x103 = x34 * x102;
	const FLT x104 = (x11 * x103) + (x6 * x98) + (-1 * x3 * x101);
	const FLT x105 = (x9 * x101) + (x6 * x103) + (-1 * x98 * x11);
	const FLT x106 = x100 + (-1 * (x36 + (2 * ((-1 * x42 * x43) + (x40 * x41))))) + (2 * ((-1 * x43 * x105) + (x41 * x104)));
	const FLT x107 = (x3 * x35) + (-1 * x9 * x39) + (x6 * x37);
	const FLT x108 = x34 * x11;
	const FLT x109 = (x3 * x98) + (x6 * x101) + (-1 * x9 * x103);
	const FLT x110 = x102 + (-1 * (x38 + (2 * ((-1 * x40 * x108) + (x43 * x107))))) + (2 * ((-1 * x108 * x104) + (x43 * x109)));
	const FLT x111 = x110 * x110;
	const FLT x112 = 1. / x111;
	const FLT x113 = x106 * x112;
	const FLT x114 = 2 * x19;
	const FLT x115 = x16 * x114;
	const FLT x116 = x15 * x114;
	const FLT x117 = -1 + x116;
	const FLT x118 = x117 + x115;
	const FLT x119 = 1. / x110;
	const FLT x120 = -1 * x110;
	const FLT x121 = atan2(x106, x120);
	const FLT x122 = x106 * x106;
	const FLT x123 = 2 * (1. / (x111 + x122)) * x111 * x121 * ((*error_model).BSD1.curve + (*_x0).BSD1.curve);
	const FLT x124 = x97 + (2 * ((-1 * x41 * x109) + (x108 * x105))) + (-1 * (x31 + (2 * ((-1 * x41 * x107) + (x42 * x108)))));
	const FLT x125 = x112 * x124;
	const FLT x126 = 2 * x3;
	const FLT x127 = x14 * x126;
	const FLT x128 = x6 * x19 * x127;
	const FLT x129 = x21 * x27;
	const FLT x130 = -1 * x129;
	const FLT x131 = x130 + x128;
	const FLT x132 = (x124 * x124) + x111;
	const FLT x133 = 1. / x132;
	const FLT x134 = x111 * x133;
	const FLT x135 = (*error_model).BSD1.tilt + (*_x0).BSD1.tilt;
	const FLT x136 = 1. / sqrt(1 + (-1 * x122 * x133 * (x135 * x135)));
	const FLT x137 = 1. / sqrt(x132);
	const FLT x138 = x137 * x135;
	const FLT x139 = 2 * x124;
	const FLT x140 = 2 * x110;
	const FLT x141 = 1.0/2.0 * x106 * (1. / (x132 * sqrt(x132))) * x135;
	const FLT x142 = (-1 * x136 * ((-1 * x141 * ((x30 * x140) + (x131 * x139))) + (x118 * x138))) + (-1 * x134 * ((x119 * x131) + (-1 * x30 * x125)));
	const FLT x143 = 1.5707963267949 + (*_x0).BSD1.gibpha + (*error_model).BSD1.gibpha + (-1 * atan2(-1 * x124, x120)) + (-1 * ((*error_model).BSD1.phase + (*_x0).BSD1.phase)) + (-1 * asin(x106 * x138));
	const FLT x144 = sin(x143) * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag);
	const FLT x145 = x6 * x22;
	const FLT x146 = x27 * x127;
	const FLT x147 = -1 * x146;
	const FLT x148 = x147 + x145;
	const FLT x149 = -1 * x128;
	const FLT x150 = x149 + x130;
	const FLT x151 = x17 * x114;
	const FLT x152 = x117 + x151;
	const FLT x153 = (-1 * x136 * ((-1 * ((x140 * x148) + (x139 * x152)) * x141) + (x138 * x150))) + (-1 * ((x119 * x152) + (-1 * x125 * x148)) * x134);
	const FLT x154 = -1 + x115 + x151;
	const FLT x155 = x24 + x28;
	const FLT x156 = -1 * x145;
	const FLT x157 = x156 + x147;
	const FLT x158 = (-1 * x136 * ((-1 * ((x140 * x154) + (x139 * x157)) * x141) + (x138 * x155))) + (-1 * ((x119 * x157) + (-1 * x125 * x154)) * x134);
	const FLT x159 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x160 = x9 * x159;
	const FLT x161 = 1.0 * x11;
	const FLT x162 = x161 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x163 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x164 = x3 * x163;
	const FLT x165 = x25 + x164 + x160 + x162;
	const FLT x166 = x32 * (1. / (x13 * sqrt(x13)));
	const FLT x167 = x166 * x165;
	const FLT x168 = x11 * x104;
	const FLT x169 = x9 * x109;
	const FLT x170 = 1. / (x13 * x13);
	const FLT x171 = x7 * x170;
	const FLT x172 = x4 * x170;
	const FLT x173 = x12 * x170;
	const FLT x174 = x10 * x170;
	const FLT x175 = x26 + (x14 * x160) + (-1 * x165 * x174) + (-1 * x165 * x172) + (-1 * x165 * x173) + (x14 * x162) + (-1 * x165 * x171) + (x14 * x164);
	const FLT x176 = x33 * (1. / (x18 * sqrt(x18)));
	const FLT x177 = x9 * x176;
	const FLT x178 = x177 * x175;
	const FLT x179 = x40 * x11;
	const FLT x180 = x9 * x107;
	const FLT x181 = x34 * x109;
	const FLT x182 = x181 * x159;
	const FLT x183 = x176 * x175;
	const FLT x184 = x34 * x107;
	const FLT x185 = x184 * x159;
	const FLT x186 = 1.0/2.0 * x11;
	const FLT x187 = x186 * x183;
	const FLT x188 = x0 * x103;
	const FLT x189 = x3 * x167;
	const FLT x190 = 1.0/2.0 * x100;
	const FLT x191 = 1.0/2.0 * x6;
	const FLT x192 = x183 * x191;
	const FLT x193 = x102 * x186;
	const FLT x194 = x97 * x167;
	const FLT x195 = x3 * x176;
	const FLT x196 = x175 * x195;
	const FLT x197 = 1.0/2.0 * x196;
	const FLT x198 = -1 * x8 * x101;
	const FLT x199 = x98 + x198;
	const FLT x200 = (-1 * x191 * x194) + x188 + (-1 * x102 * x187) + (-1 * x97 * x192) + (x100 * x197) + (x189 * x190) + x199 + (-1 * x167 * x193);
	const FLT x201 = x34 * x20;
	const FLT x202 = 1.0/2.0 * x36;
	const FLT x203 = x167 * x186;
	const FLT x204 = x167 * x191;
	const FLT x205 = x0 * x39;
	const FLT x206 = -1 * x8 * x37;
	const FLT x207 = x206 + x35;
	const FLT x208 = x205 + (-1 * x38 * x187) + (-1 * x31 * x192) + (x202 * x189) + (-1 * x38 * x203) + (x36 * x197) + x207 + (-1 * x31 * x204);
	const FLT x209 = 1.0/2.0 * x31;
	const FLT x210 = x8 * x35;
	const FLT x211 = 1.0/2.0 * x178;
	const FLT x212 = 1.0/2.0 * x38;
	const FLT x213 = x9 * x167;
	const FLT x214 = -1 * x2 * x39;
	const FLT x215 = x214 + x37;
	const FLT x216 = x215 + (x213 * x212) + (-1 * x36 * x192) + x210 + (-1 * x209 * x189) + (x38 * x211) + (-1 * x31 * x197) + (-1 * x36 * x204);
	const FLT x217 = 2 * x9;
	const FLT x218 = x34 * x217;
	const FLT x219 = 1.0/2.0 * x97;
	const FLT x220 = x8 * x98;
	const FLT x221 = -1 * x2 * x103;
	const FLT x222 = 1.0/2.0 * x102;
	const FLT x223 = x9 * x222;
	const FLT x224 = (x211 * x102) + x101 + (x223 * x167) + x221 + (-1 * x219 * x189) + (-1 * x97 * x197) + (-1 * x100 * x192) + x220 + (-1 * x204 * x100);
	const FLT x225 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x226 = x34 * x104;
	const FLT x227 = x40 * x34;
	const FLT x228 = (x227 * x225) + (-1 * x225 * x226);
	const FLT x229 = x228 + (-1 * x218 * x216) + (x167 * x180) + (x218 * x224) + (x201 * x208) + (-1 * x167 * x179) + (-1 * x185) + (-1 * x109 * x178) + x182 + (-1 * x169 * x167) + (x180 * x183) + (-1 * x179 * x183) + (-1 * x200 * x201) + (x168 * x167) + (x168 * x183);
	const FLT x230 = x34 * x126;
	const FLT x231 = x226 * x163;
	const FLT x232 = x3 * x40;
	const FLT x233 = x227 * x163;
	const FLT x234 = x9 * x105;
	const FLT x235 = x2 * x37;
	const FLT x236 = -1 * x0 * x35;
	const FLT x237 = x236 + (x31 * x187) + x235 + (-1 * x38 * x204) + (-1 * x36 * x211) + (-1 * x213 * x202) + (x31 * x203) + x39 + (-1 * x38 * x192);
	const FLT x238 = x9 * x42;
	const FLT x239 = x9 * x190;
	const FLT x240 = x102 * x191;
	const FLT x241 = x2 * x101;
	const FLT x242 = -1 * x0 * x98;
	const FLT x243 = x242 + x103;
	const FLT x244 = x243 + x241 + (-1 * x240 * x167) + (-1 * x211 * x100) + (-1 * x239 * x167) + (x97 * x187) + (x186 * x194) + (-1 * x102 * x192);
	const FLT x245 = x42 * x34;
	const FLT x246 = x34 * x105;
	const FLT x247 = (-1 * x246 * x159) + (x245 * x159);
	const FLT x248 = (-1 * x104 * x196) + (-1 * x104 * x189) + x247 + (x105 * x178) + (-1 * x238 * x183) + (-1 * x238 * x167) + (x230 * x200) + (-1 * x230 * x208) + (-1 * x218 * x244) + x231 + (-1 * x233) + (x232 * x167) + (x234 * x167) + (x218 * x237) + (x40 * x196);
	const FLT x249 = x42 * x11;
	const FLT x250 = x225 * x245;
	const FLT x251 = x11 * x105;
	const FLT x252 = x225 * x246;
	const FLT x253 = (x163 * x184) + (-1 * x163 * x181);
	const FLT x254 = (-1 * x237 * x201) + (x249 * x183) + x253 + (x249 * x167) + (x201 * x244) + (-1 * x107 * x189) + (-1 * x251 * x167) + (-1 * x250) + (-1 * x251 * x183) + (-1 * x230 * x224) + (-1 * x107 * x196) + x252 + (x109 * x196) + (x216 * x230) + (x109 * x189);
	const FLT x255 = (-1 * x136 * ((-1 * ((x229 * x140) + (x254 * x139)) * x141) + (x248 * x138))) + (-1 * ((x254 * x119) + (-1 * x229 * x125)) * x134);
	const FLT x256 = x3 * x159;
	const FLT x257 = x9 * x163;
	const FLT x258 = x6 * x225;
	const FLT x259 = (-1 * x258) + x256 + (-1 * x257) + x20;
	const FLT x260 = x259 * x166;
	const FLT x261 = -1 * x241;
	const FLT x262 = x11 * x176;
	const FLT x263 = x259 * x170;
	const FLT x264 = (-1 * x10 * x263) + x21 + (-1 * x12 * x263) + (-1 * x4 * x263) + (-1 * x14 * x257) + (-1 * x7 * x263) + (-1 * x14 * x258) + (x14 * x256);
	const FLT x265 = x264 * x222;
	const FLT x266 = x260 * x186;
	const FLT x267 = x3 * x260;
	const FLT x268 = x264 * x176;
	const FLT x269 = x97 * x191;
	const FLT x270 = x264 * x195;
	const FLT x271 = (x270 * x190) + (-1 * x265 * x262) + x261 + (-1 * x269 * x260) + (-1 * x266 * x102) + x243 + (-1 * x268 * x269) + (x267 * x190);
	const FLT x272 = x264 * x109;
	const FLT x273 = 2 * x34;
	const FLT x274 = x273 * x104;
	const FLT x275 = x40 * x273;
	const FLT x276 = x260 * x191;
	const FLT x277 = x264 * x262;
	const FLT x278 = x268 * x191;
	const FLT x279 = (-1 * x235) + x236;
	const FLT x280 = (-1 * x31 * x278) + (x202 * x270) + x39 + (-1 * x38 * x266) + (x202 * x267) + x279 + (-1 * x31 * x276) + (-1 * x212 * x277);
	const FLT x281 = x2 * x35;
	const FLT x282 = x264 * x177;
	const FLT x283 = x8 * x39;
	const FLT x284 = x9 * x260;
	const FLT x285 = x0 * x37;
	const FLT x286 = (-1 * x36 * x278) + (-1 * x285) + (-1 * x36 * x276) + (-1 * x209 * x267) + (x212 * x282) + (-1 * x209 * x270) + (x212 * x284) + x281 + x283;
	const FLT x287 = x264 * x104;
	const FLT x288 = x2 * x98;
	const FLT x289 = x0 * x101;
	const FLT x290 = x8 * x103;
	const FLT x291 = x290 + (x265 * x177) + (-1 * x278 * x100) + (x260 * x223) + (-1 * x289) + (-1 * x219 * x270) + (-1 * x219 * x267) + x288 + (-1 * x276 * x100);
	const FLT x292 = x253 + (x260 * x168) + (x268 * x180) + (x218 * x291) + x275 + (-1 * x268 * x179) + (-1 * x260 * x179) + (-1 * x274) + (-1 * x260 * x169) + (-1 * x201 * x271) + (-1 * x218 * x286) + (-1 * x272 * x177) + (x262 * x287) + (x201 * x280) + (x260 * x180);
	const FLT x293 = -1 * x205;
	const FLT x294 = (x209 * x277) + (-1 * x202 * x282) + x206 + (-1 * x38 * x278) + (-1 * x35) + (-1 * x202 * x284) + (x31 * x266) + (-1 * x38 * x276) + x293;
	const FLT x295 = x264 * x105;
	const FLT x296 = -1 * x188;
	const FLT x297 = x198 + (x219 * x277) + (-1 * x282 * x190) + (-1 * x239 * x260) + (-1 * x260 * x240) + (x97 * x266) + x296 + (-1 * x98) + (-1 * x268 * x240);
	const FLT x298 = (x226 * x159) + (-1 * x227 * x159);
	const FLT x299 = (-1 * x245 * x163) + (x246 * x163);
	const FLT x300 = x298 + (-1 * x218 * x297) + (x40 * x270) + x299 + (-1 * x287 * x195) + (-1 * x238 * x268) + (-1 * x238 * x260) + (-1 * x267 * x104) + (x232 * x260) + (-1 * x230 * x280) + (x295 * x177) + (x218 * x294) + (x230 * x271) + (x234 * x260);
	const FLT x301 = x273 * x105;
	const FLT x302 = x42 * x273;
	const FLT x303 = (-1 * x230 * x291) + (-1 * x182) + (-1 * x302) + (x272 * x195) + (-1 * x201 * x294) + (x201 * x297) + (x260 * x249) + (-1 * x267 * x107) + (x267 * x109) + (-1 * x270 * x107) + (x230 * x286) + (-1 * x295 * x262) + x185 + x301 + (x268 * x249) + (-1 * x260 * x251);
	const FLT x304 = (-1 * x136 * ((-1 * ((x292 * x140) + (x303 * x139)) * x141) + (x300 * x138))) + (-1 * ((x303 * x119) + (-1 * x292 * x125)) * x134);
	const FLT x305 = x161 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x306 = x3 * x225;
	const FLT x307 = x6 * x159;
	const FLT x308 = (-1 * x307) + (-1 * x306) + x217 + x305;
	const FLT x309 = x308 * x166;
	const FLT x310 = ((x14 * x217) + (-1 * x308 * x174) + (-1 * x308 * x173) + (x14 * x305) + (-1 * x308 * x171) + (-1 * x14 * x307) + (-1 * x308 * x172) + (-1 * x14 * x306)) * x176;
	const FLT x311 = x3 * x309;
	const FLT x312 = x309 * x191;
	const FLT x313 = x3 * x310;
	const FLT x314 = (x313 * x190) + x289 + (-1 * x97 * x312) + (-1 * x288) + x290 + (-1 * x310 * x193) + (-1 * x309 * x193) + (x311 * x190) + (-1 * x269 * x310);
	const FLT x315 = x273 * x109;
	const FLT x316 = x9 * x310;
	const FLT x317 = x9 * x309;
	const FLT x318 = x310 * x191;
	const FLT x319 = x279 + (-1 * x209 * x311) + (x212 * x316) + (-1 * x36 * x312) + (-1 * x209 * x313) + (-1 * x39) + (-1 * x36 * x318) + (x212 * x317);
	const FLT x320 = x309 * x186;
	const FLT x321 = x310 * x186;
	const FLT x322 = x285 + (-1 * x281) + x283 + (x202 * x313) + (-1 * x38 * x320) + (-1 * x31 * x312) + (-1 * x38 * x321) + (-1 * x31 * x318) + (x202 * x311);
	const FLT x323 = x273 * x107;
	const FLT x324 = (-1 * x103) + (-1 * x219 * x311) + (-1 * x312 * x100) + x242 + (x222 * x316) + (-1 * x219 * x313) + (x222 * x317) + (-1 * x318 * x100) + x261;
	const FLT x325 = (x309 * x168) + (-1 * x309 * x179) + (-1 * x316 * x109) + x233 + x315 + (x309 * x180) + (-1 * x231) + (-1 * x218 * x319) + (-1 * x323) + (-1 * x309 * x169) + (-1 * x201 * x314) + (x218 * x324) + (x310 * x168) + (x310 * x180) + (-1 * x310 * x179) + (x201 * x322);
	const FLT x326 = 1.0/2.0 * x317;
	const FLT x327 = -1 * x210;
	const FLT x328 = x215 + x327 + (x31 * x321) + (-1 * x38 * x318) + (-1 * x202 * x316) + (x31 * x320) + (-1 * x38 * x312) + (-1 * x36 * x326);
	const FLT x329 = x221 + (-1 * x220);
	const FLT x330 = x329 + x101 + (-1 * x240 * x309) + (x97 * x321) + (-1 * x326 * x100) + (-1 * x316 * x190) + (x97 * x320) + (-1 * x240 * x310);
	const FLT x331 = x228 + (x316 * x105) + (x234 * x309) + (-1 * x238 * x309) + (-1 * x313 * x104) + (x218 * x328) + (x40 * x311) + (x232 * x310) + (-1 * x301) + (-1 * x238 * x310) + (-1 * x311 * x104) + (-1 * x230 * x322) + (-1 * x218 * x330) + (x230 * x314) + x302;
	const FLT x332 = (-1 * x225 * x184) + (x225 * x181);
	const FLT x333 = x299 + (x249 * x309) + (-1 * x201 * x328) + (x230 * x319) + (x249 * x310) + (x311 * x109) + (-1 * x230 * x324) + x332 + (-1 * x311 * x107) + (-1 * x251 * x310) + (x201 * x330) + (x313 * x109) + (-1 * x251 * x309) + (-1 * x313 * x107);
	const FLT x334 = (-1 * x136 * ((-1 * ((x325 * x140) + (x333 * x139)) * x141) + (x331 * x138))) + (-1 * ((x333 * x119) + (-1 * x325 * x125)) * x134);
	const FLT x335 = x11 * x159;
	const FLT x336 = x9 * x225;
	const FLT x337 = x6 * x163;
	const FLT x338 = (-1 * x337) + (-1 * x335) + x126 + x336;
	const FLT x339 = x338 * x166;
	const FLT x340 = (-1 * x14 * x337) + (-1 * x338 * x172) + (x14 * x336) + (-1 * x338 * x173) + (-1 * x14 * x335) + (-1 * x338 * x171) + x127 + (-1 * x338 * x174);
	const FLT x341 = x262 * x340;
	const FLT x342 = x3 * x339;
	const FLT x343 = 1.0/2.0 * x342;
	const FLT x344 = x340 * x176;
	const FLT x345 = x340 * x195;
	const FLT x346 = (-1 * x101) + (-1 * x339 * x193) + x329 + (x343 * x100) + (-1 * x269 * x339) + (x345 * x190) + (-1 * x222 * x341) + (-1 * x269 * x344);
	const FLT x347 = x344 * x191;
	const FLT x348 = x339 * x191;
	const FLT x349 = x340 * x177;
	const FLT x350 = (x222 * x349) + (x223 * x339) + x296 + x199 + (-1 * x219 * x342) + (-1 * x347 * x100) + (-1 * x219 * x345) + (-1 * x348 * x100);
	const FLT x351 = x339 * x186;
	const FLT x352 = (-1 * x37) + (x202 * x345) + (-1 * x212 * x341) + (-1 * x31 * x347) + x214 + (-1 * x38 * x351) + x327 + (-1 * x31 * x348) + (x36 * x343);
	const FLT x353 = x9 * x339;
	const FLT x354 = x207 + (-1 * x36 * x347) + (x212 * x353) + (-1 * x36 * x348) + (-1 * x31 * x343) + (-1 * x209 * x345) + (x212 * x349) + x293;
	const FLT x355 = (x341 * x104) + (x201 * x352) + (-1 * x339 * x179) + (x218 * x350) + (-1 * x339 * x169) + x298 + (-1 * x201 * x346) + (x339 * x180) + x332 + (-1 * x344 * x179) + (-1 * x218 * x354) + (x339 * x168) + (-1 * x349 * x109) + (x344 * x180);
	const FLT x356 = (-1 * x38 * x347) + x281 + (-1 * x283) + x285 + (-1 * x202 * x349) + (x31 * x351) + (x209 * x341) + (-1 * x38 * x348) + (-1 * x202 * x353);
	const FLT x357 = x340 * x105;
	const FLT x358 = (-1 * x290) + (-1 * x349 * x190) + x288 + (x97 * x351) + (-1 * x240 * x344) + x289 + (-1 * x353 * x190) + (x219 * x341) + (-1 * x240 * x339);
	const FLT x359 = (x234 * x339) + (x357 * x177) + (-1 * x345 * x104) + (-1 * x230 * x352) + (x40 * x345) + (-1 * x342 * x104) + (x40 * x342) + x250 + x274 + (x230 * x346) + (-1 * x252) + (-1 * x275) + (-1 * x238 * x344) + (-1 * x218 * x358) + (-1 * x238 * x339) + (x218 * x356);
	const FLT x360 = x247 + x323 + (-1 * x315) + (x342 * x109) + (-1 * x251 * x339) + (-1 * x342 * x107) + (-1 * x262 * x357) + (x230 * x354) + (x249 * x339) + (x201 * x358) + (-1 * x345 * x107) + (x345 * x109) + (-1 * x201 * x356) + (x249 * x344) + (-1 * x230 * x350);
	const FLT x361 = (-1 * x136 * ((-1 * ((x355 * x140) + (x360 * x139)) * x141) + (x359 * x138))) + (-1 * ((x360 * x119) + (-1 * x355 * x125)) * x134);
	const FLT x362 = x44 * x19;
	const FLT x363 = x14 * x362;
	const FLT x364 = x9 * x363;
	const FLT x365 = x6 * x364;
	const FLT x366 = x3 * x11 * x363;
	const FLT x367 = x366 + x365;
	const FLT x368 = -1 * x16 * x362;
	const FLT x369 = -1 * x15 * x362;
	const FLT x370 = x45 + x369 + x368;
	const FLT x371 = x11 * x364;
	const FLT x372 = x6 * x363;
	const FLT x373 = x3 * x372;
	const FLT x374 = (-1 * x373) + x371;
	const FLT x375 = (-1 * x136 * ((-1 * ((x367 * x140) + (x374 * x139)) * x141) + (x370 * x138))) + (-1 * ((x374 * x119) + (-1 * x367 * x125)) * x134);
	const FLT x376 = x3 * x364;
	const FLT x377 = x11 * x372;
	const FLT x378 = (-1 * x377) + x376;
	const FLT x379 = x371 + x373;
	const FLT x380 = (-1 * x17 * x362) + x45;
	const FLT x381 = x380 + x369;
	const FLT x382 = (-1 * x136 * ((-1 * ((x378 * x140) + (x381 * x139)) * x141) + (x379 * x138))) + (-1 * ((x381 * x119) + (-1 * x378 * x125)) * x134);
	const FLT x383 = x380 + x368;
	const FLT x384 = (-1 * x365) + x366;
	const FLT x385 = x376 + x377;
	const FLT x386 = (-1 * x136 * ((-1 * ((x383 * x140) + (x385 * x139)) * x141) + (x384 * x138))) + (-1 * ((x385 * x119) + (-1 * x383 * x125)) * x134);
	const FLT x387 = x23 + x28;
	const FLT x388 = -1 * x115;
	const FLT x389 = 1 + (-1 * x116);
	const FLT x390 = x389 + x388;
	const FLT x391 = x149 + x129;
	const FLT x392 = (-1 * x136 * ((-1 * ((x387 * x140) + (x391 * x139)) * x141) + (x390 * x138))) + (-1 * ((x391 * x119) + (-1 * x387 * x125)) * x134);
	const FLT x393 = x156 + x146;
	const FLT x394 = x129 + x128;
	const FLT x395 = -1 * x151;
	const FLT x396 = x389 + x395;
	const FLT x397 = (-1 * x136 * ((-1 * ((x393 * x140) + (x396 * x139)) * x141) + (x394 * x138))) + (-1 * ((x396 * x119) + (-1 * x393 * x125)) * x134);
	const FLT x398 = 1 + x395 + x388;
	const FLT x399 = x29 + x23;
	const FLT x400 = x146 + x145;
	const FLT x401 = (-1 * x136 * ((-1 * ((x398 * x140) + (x400 * x139)) * x141) + (x399 * x138))) + (-1 * ((x400 * x119) + (-1 * x398 * x125)) * x134);
	const FLT x402 = 0.5 * x83;
	const FLT x403 = x402 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x404 = x80 * x61;
	const FLT x405 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x406 = x405 * x404;
	const FLT x407 = 1.0 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x408 = 1.0 * x52;
	const FLT x409 = 1.0 * x50;
	const FLT x410 = x76 * (1. / (x56 * sqrt(x56)));
	const FLT x411 = x410 * ((x409 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x408 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x55 * x407) + (2 * x54));
	const FLT x412 = 1.0/2.0 * x79;
	const FLT x413 = x61 * x412;
	const FLT x414 = x413 * x411;
	const FLT x415 = 1.0/2.0 * x74;
	const FLT x416 = x52 * x415;
	const FLT x417 = x46 * x412;
	const FLT x418 = x411 * x417;
	const FLT x419 = x64 * x411;
	const FLT x420 = x412 * x419;
	const FLT x421 = x80 * x64;
	const FLT x422 = -1 * x53 * x421;
	const FLT x423 = x80 * x46;
	const FLT x424 = x423 + x422;
	const FLT x425 = x424 + (x55 * x420) + x406 + x403 + (-1 * x50 * x414) + (-1 * x54 * x418) + (-1 * x411 * x416);
	const FLT x426 = 2 * x99;
	const FLT x427 = x50 * x415;
	const FLT x428 = x53 * x423;
	const FLT x429 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x430 = -1 * x404 * x429;
	const FLT x431 = x402 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x432 = x431 + x430;
	const FLT x433 = x432 + (-1 * x55 * x418) + x421 + (-1 * x54 * x420) + x428 + (x52 * x414) + (-1 * x411 * x427);
	const FLT x434 = x53 * x404;
	const FLT x435 = -1 * x434;
	const FLT x436 = x54 * x415;
	const FLT x437 = x52 * x417;
	const FLT x438 = x405 * x421;
	const FLT x439 = -1 * x438;
	const FLT x440 = x50 * x412;
	const FLT x441 = x423 * x429;
	const FLT x442 = x83 + (-1 * x441);
	const FLT x443 = (x419 * x440) + (-1 * x411 * x436) + x442 + x435 + (x55 * x414) + x439 + (x411 * x437);
	const FLT x444 = x83 * x53;
	const FLT x445 = x421 * x429;
	const FLT x446 = x55 * x415;
	const FLT x447 = x46 * x440;
	const FLT x448 = -1 * x405 * x423;
	const FLT x449 = x404 + x448;
	const FLT x450 = (-1 * x52 * x420) + x445 + (-1 * x411 * x446) + x449 + x444 + (-1 * x54 * x414) + (x411 * x447);
	const FLT x451 = (x450 * sensor_pt[1]) + (-1 * x433 * sensor_pt[0]) + (x443 * sensor_pt[2]);
	const FLT x452 = 2 * x92;
	const FLT x453 = (x425 * sensor_pt[0]) + (-1 * x450 * sensor_pt[2]) + (x443 * sensor_pt[1]);
	const FLT x454 = 2 * x95;
	const FLT x455 = 2 * x93;
	const FLT x456 = (x455 * x433) + (-1 * x453 * x454) + (-1 * x425 * x426) + (x451 * x452);
	const FLT x457 = x6 * x34;
	const FLT x458 = 2 * x86;
	const FLT x459 = 2 * x96;
	const FLT x460 = (x433 * sensor_pt[2]) + (x443 * sensor_pt[0]) + (-1 * x425 * sensor_pt[1]);
	const FLT x461 = (x454 * x460) + (x459 * x425) + (-1 * x455 * x450) + (-1 * x451 * x458);
	const FLT x462 = (x453 * x458) + (x450 * x426) + (-1 * x459 * x433) + (-1 * x452 * x460);
	const FLT x463 = (x456 * x457) + (-1 * x43 * x462) + (x41 * x461);
	const FLT x464 = (x462 * x108) + (-1 * x41 * x456) + (x457 * x461);
	const FLT x465 = x462 + (x463 * x218) + (-1 * x464 * x201);
	const FLT x466 = (-1 * x461 * x108) + (x43 * x456) + (x457 * x462);
	const FLT x467 = x456 + (-1 * x466 * x218) + (x464 * x230);
	const FLT x468 = x461 + (x466 * x201) + (-1 * x463 * x230);
	const FLT x469 = (-1 * x136 * ((-1 * ((x465 * x140) + (x468 * x139)) * x141) + (x467 * x138))) + (-1 * ((x468 * x119) + (-1 * x465 * x125)) * x134);
	const FLT x470 = 1.0 * x54;
	const FLT x471 = x410 * ((-1 * x409 * (*error_model).Object.Pose.AxisAngleRot[2]) + (2 * x55) + (-1 * x470 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x408 * (*error_model).Object.Pose.AxisAngleRot[1]));
	const FLT x472 = x471 * x413;
	const FLT x473 = x64 * x412;
	const FLT x474 = x473 * x471;
	const FLT x475 = x471 * x415;
	const FLT x476 = x441 + x83;
	const FLT x477 = x476 + x435 + (-1 * x55 * x475) + x438 + (-1 * x54 * x472) + (x471 * x447) + (-1 * x52 * x474);
	const FLT x478 = -1 * x406;
	const FLT x479 = -1 * x403;
	const FLT x480 = x54 * x473;
	const FLT x481 = x471 * x417;
	const FLT x482 = (-1 * x50 * x475) + (x52 * x472) + x479 + x478 + (-1 * x471 * x480) + x424 + (-1 * x55 * x481);
	const FLT x483 = x64 * x440;
	const FLT x484 = -1 * x444;
	const FLT x485 = x448 + (-1 * x404);
	const FLT x486 = x484 + (-1 * x471 * x436) + (x55 * x472) + x445 + (x52 * x481) + x485 + (x471 * x483);
	const FLT x487 = (x477 * sensor_pt[1]) + (-1 * x482 * sensor_pt[0]) + (x486 * sensor_pt[2]);
	const FLT x488 = -1 * x428;
	const FLT x489 = -1 * x421;
	const FLT x490 = (-1 * x52 * x475) + x432 + (-1 * x50 * x472) + (x55 * x474) + x488 + (-1 * x54 * x481) + x489;
	const FLT x491 = (x482 * sensor_pt[2]) + (-1 * x490 * sensor_pt[1]) + (x486 * sensor_pt[0]);
	const FLT x492 = (x491 * x454) + (x490 * x459) + (-1 * x455 * x477) + (-1 * x458 * x487);
	const FLT x493 = (x490 * sensor_pt[0]) + (x486 * sensor_pt[1]) + (-1 * x477 * sensor_pt[2]);
	const FLT x494 = (-1 * x493 * x454) + (x455 * x482) + (-1 * x490 * x426) + (x452 * x487);
	const FLT x495 = (x493 * x458) + (x477 * x426) + (-1 * x459 * x482) + (-1 * x491 * x452);
	const FLT x496 = (-1 * x43 * x495) + (x41 * x492) + (x494 * x457);
	const FLT x497 = (-1 * x41 * x494) + (x495 * x108) + (x492 * x457);
	const FLT x498 = x495 + (x496 * x218) + (-1 * x497 * x201);
	const FLT x499 = (-1 * x492 * x108) + (x43 * x494) + (x495 * x457);
	const FLT x500 = (x497 * x230) + x494 + (-1 * x499 * x218);
	const FLT x501 = x492 + (-1 * x496 * x230) + (x499 * x201);
	const FLT x502 = (-1 * x136 * ((-1 * ((x498 * x140) + (x501 * x139)) * x141) + (x500 * x138))) + (-1 * ((x501 * x119) + (-1 * x498 * x125)) * x134);
	const FLT x503 = 1.0 * x55;
	const FLT x504 = x410 * ((2 * x50) + (x503 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x470 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x52 * x407));
	const FLT x505 = x413 * x504;
	const FLT x506 = x473 * x504;
	const FLT x507 = (-1 * x423) + x422;
	const FLT x508 = x507 + (-1 * x52 * x506) + (-1 * x446 * x504) + x478 + (-1 * x54 * x505) + (x447 * x504) + x403;
	const FLT x509 = -1 * x445;
	const FLT x510 = x415 * x504;
	const FLT x511 = x417 * x504;
	const FLT x512 = x449 + (-1 * x54 * x511) + x484 + x509 + (-1 * x50 * x505) + (-1 * x52 * x510) + (x55 * x506);
	const FLT x513 = (-1 * x431) + x430;
	const FLT x514 = x513 + (x55 * x505) + x489 + x428 + (-1 * x436 * x504) + (x52 * x511) + (x483 * x504);
	const FLT x515 = x476 + (-1 * x50 * x510) + x439 + (x52 * x505) + (-1 * x55 * x511) + x434 + (-1 * x54 * x506);
	const FLT x516 = 2 * ((x515 * sensor_pt[2]) + (-1 * x512 * sensor_pt[1]) + (x514 * sensor_pt[0]));
	const FLT x517 = (x508 * sensor_pt[1]) + (-1 * x515 * sensor_pt[0]) + (x514 * sensor_pt[2]);
	const FLT x518 = (-1 * x458 * x517) + (x459 * x512) + (-1 * x455 * x508) + (x95 * x516);
	const FLT x519 = (-1 * x508 * sensor_pt[2]) + (x512 * sensor_pt[0]) + (x514 * sensor_pt[1]);
	const FLT x520 = (-1 * x454 * x519) + (-1 * x426 * x512) + (x455 * x515) + (x452 * x517);
	const FLT x521 = (-1 * x92 * x516) + (-1 * x459 * x515) + (x458 * x519) + (x426 * x508);
	const FLT x522 = (x41 * x518) + (-1 * x43 * x521) + (x457 * x520);
	const FLT x523 = (x521 * x108) + (-1 * x41 * x520) + (x457 * x518);
	const FLT x524 = (x522 * x218) + x521 + (-1 * x523 * x201);
	const FLT x525 = (-1 * x518 * x108) + (x43 * x520) + (x457 * x521);
	const FLT x526 = (x523 * x230) + x520 + (-1 * x525 * x218);
	const FLT x527 = x518 + (x525 * x201) + (-1 * x522 * x230);
	const FLT x528 = (-1 * x136 * ((-1 * ((x524 * x140) + (x527 * x139)) * x141) + (x526 * x138))) + (-1 * ((x527 * x119) + (-1 * x524 * x125)) * x134);
	const FLT x529 = x410 * ((2 * x52) + (x50 * x407) + (-1 * x503 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x470 * (*error_model).Object.Pose.AxisAngleRot[2]));
	const FLT x530 = x413 * x529;
	const FLT x531 = x46 * x529;
	const FLT x532 = x64 * x529;
	const FLT x533 = x412 * x532;
	const FLT x534 = x513 + (-1 * x52 * x533) + x488 + x421 + (-1 * x446 * x529) + (-1 * x54 * x530) + (x440 * x531);
	const FLT x535 = x412 * x531;
	const FLT x536 = x479 + (-1 * x436 * x529) + x507 + x406 + (x55 * x530) + (x440 * x532) + (x52 * x535);
	const FLT x537 = x485 + (-1 * x54 * x533) + (-1 * x427 * x529) + (-1 * x55 * x535) + (x52 * x530) + x444 + x509;
	const FLT x538 = (x534 * sensor_pt[1]) + (x536 * sensor_pt[2]) + (-1 * x537 * sensor_pt[0]);
	const FLT x539 = x442 + x438 + (-1 * x50 * x530) + x434 + (-1 * x416 * x529) + (-1 * x54 * x535) + (x55 * x533);
	const FLT x540 = (x537 * sensor_pt[2]) + (-1 * x539 * sensor_pt[1]) + (x536 * sensor_pt[0]);
	const FLT x541 = (x454 * x540) + (x459 * x539) + (-1 * x455 * x534) + (-1 * x458 * x538);
	const FLT x542 = (x539 * sensor_pt[0]) + (-1 * x534 * sensor_pt[2]) + (x536 * sensor_pt[1]);
	const FLT x543 = (-1 * x454 * x542) + (x455 * x537) + (-1 * x426 * x539) + (x452 * x538);
	const FLT x544 = (x458 * x542) + (x426 * x534) + (-1 * x459 * x537) + (-1 * x452 * x540);
	const FLT x545 = (x41 * x541) + (-1 * x43 * x544) + (x457 * x543);
	const FLT x546 = (-1 * x41 * x543) + (x544 * x108) + (x457 * x541);
	const FLT x547 = x544 + (x545 * x218) + (-1 * x546 * x201);
	const FLT x548 = (-1 * x541 * x108) + (x43 * x543) + (x457 * x544);
	const FLT x549 = x543 + (x546 * x230) + (-1 * x548 * x218);
	const FLT x550 = x541 + (-1 * x545 * x230) + (x548 * x201);
	const FLT x551 = (-1 * x136 * ((-1 * ((x547 * x140) + (x550 * x139)) * x141) + (x549 * x138))) + (-1 * ((x550 * x119) + (-1 * x547 * x125)) * x134);
	const FLT x552 = dt * dt * dt;
	const FLT x553 = 0.5 * x72 * x552;
	const FLT x554 = x61 * x553;
	const FLT x555 = x46 * x554;
	const FLT x556 = x91 * x555;
	const FLT x557 = x64 * x554;
	const FLT x558 = x94 * x557;
	const FLT x559 = x62 * x553;
	const FLT x560 = x61 * x61 * x61;
	const FLT x561 = dt * dt * dt * dt;
	const FLT x562 = 2 * x71 * (1. / (x67 * x67));
	const FLT x563 = x561 * x562;
	const FLT x564 = 1.0 * x74;
	const FLT x565 = x70 * (1. / (x67 * sqrt(x67)));
	const FLT x566 = x564 * x565;
	const FLT x567 = x561 * x566;
	const FLT x568 = x61 * x567;
	const FLT x569 = x61 * x563;
	const FLT x570 = 2 * x73;
	const FLT x571 = x58 * x570;
	const FLT x572 = x78 * x564;
	const FLT x573 = x58 * x572;
	const FLT x574 = (1. / (x75 * sqrt(x75))) * x57;
	const FLT x575 = ((-1 * x61 * x573) + (x560 * x567) + (x65 * x568) + (x59 * x568) + (-1 * x560 * x563) + (-1 * x59 * x569) + (-1 * x65 * x569) + (x61 * x571)) * x574;
	const FLT x576 = x52 * x575;
	const FLT x577 = 0.5 * x78;
	const FLT x578 = x58 * x577;
	const FLT x579 = x88 * x578;
	const FLT x580 = x77 * x52;
	const FLT x581 = x61 * x580;
	const FLT x582 = x552 * x565;
	const FLT x583 = x64 * x582;
	const FLT x584 = x581 * x583;
	const FLT x585 = x413 * x575;
	const FLT x586 = x77 * x50;
	const FLT x587 = x46 * x582;
	const FLT x588 = x61 * x587;
	const FLT x589 = x586 * x588;
	const FLT x590 = x77 * x54;
	const FLT x591 = x62 * x582;
	const FLT x592 = (-1 * x446 * x575) + x558 + (x87 * x559) + x85 + x589 + (-1 * x556) + (-1 * x591 * x590) + (x447 * x575) + (-1 * x473 * x576) + (-1 * x61 * x579) + (-1 * x584) + (-1 * x54 * x585);
	const FLT x593 = x417 * x575;
	const FLT x594 = x61 * x583;
	const FLT x595 = (-1 * x84 * x557) + (x88 * x594);
	const FLT x596 = x61 * x590;
	const FLT x597 = (-1 * x596 * x587) + (x87 * x555);
	const FLT x598 = x597 + (-1 * x591 * x586) + (-1 * x416 * x575) + (-1 * x578 * x581) + (x91 * x559) + x81 + (x55 * x473 * x575) + x595 + (-1 * x50 * x585) + (-1 * x54 * x593);
	const FLT x599 = x594 * x586;
	const FLT x600 = x91 * x557;
	const FLT x601 = -1 * x89;
	const FLT x602 = (x581 * x587) + (-1 * x94 * x555);
	const FLT x603 = x602 + (-1 * x436 * x575) + x601 + (x88 * x591) + x599 + (x55 * x585) + (x483 * x575) + (-1 * x596 * x578) + (-1 * x84 * x559) + (-1 * x600) + (x417 * x576);
	const FLT x604 = -1 * x82;
	const FLT x605 = x88 * x588;
	const FLT x606 = x84 * x555;
	const FLT x607 = (-1 * x596 * x583) + (x87 * x557);
	const FLT x608 = (x413 * x576) + (-1 * x605) + (-1 * x427 * x575) + x606 + (-1 * x61 * x578 * x586) + (-1 * x55 * x593) + x604 + x607 + (x591 * x580) + (-1 * x480 * x575) + (-1 * x94 * x559);
	const FLT x609 = 2 * ((-1 * x598 * sensor_pt[1]) + (x608 * sensor_pt[2]) + (x603 * sensor_pt[0]));
	const FLT x610 = (x592 * sensor_pt[1]) + (-1 * x608 * sensor_pt[0]) + (x603 * sensor_pt[2]);
	const FLT x611 = (x95 * x609) + (-1 * x455 * x592) + (-1 * x458 * x610) + (x459 * x598);
	const FLT x612 = 2 * ((x598 * sensor_pt[0]) + (-1 * x592 * sensor_pt[2]) + (x603 * sensor_pt[1]));
	const FLT x613 = (-1 * x95 * x612) + (x455 * x608) + (-1 * x426 * x598) + (x452 * x610);
	const FLT x614 = (x86 * x612) + (x426 * x592) + (-1 * x459 * x608) + (-1 * x92 * x609);
	const FLT x615 = (-1 * x43 * x614) + (x41 * x611) + (x457 * x613);
	const FLT x616 = (-1 * x41 * x613) + (x614 * x108) + (x457 * x611);
	const FLT x617 = x614 + (x615 * x218) + (-1 * x616 * x201);
	const FLT x618 = (-1 * x611 * x108) + (x43 * x613) + (x457 * x614);
	const FLT x619 = x613 + (x616 * x230) + (-1 * x618 * x218);
	const FLT x620 = x611 + (x618 * x201) + (-1 * x615 * x230);
	const FLT x621 = (-1 * x136 * ((-1 * ((x617 * x140) + (x620 * x139)) * x141) + (x619 * x138))) + (-1 * ((x620 * x119) + (-1 * x617 * x125)) * x134);
	const FLT x622 = x65 * x553;
	const FLT x623 = x65 * x582;
	const FLT x624 = x62 * x563;
	const FLT x625 = x62 * x567;
	const FLT x626 = x64 * x58;
	const FLT x627 = x64 * x59;
	const FLT x628 = (x64 * x64 * x64) * x561;
	const FLT x629 = ((x626 * x570) + (x628 * x566) + (x64 * x625) + (-1 * x64 * x624) + (-1 * x627 * x563) + (-1 * x626 * x572) + (-1 * x628 * x562) + (x627 * x567)) * x574;
	const FLT x630 = x413 * x629;
	const FLT x631 = x417 * x629;
	const FLT x632 = x64 * x629;
	const FLT x633 = x412 * x632;
	const FLT x634 = x415 * x629;
	const FLT x635 = x626 * x577;
	const FLT x636 = x64 * x46 * x553;
	const FLT x637 = x64 * x587;
	const FLT x638 = (-1 * x637 * x590) + (x87 * x636);
	const FLT x639 = x638 + (x55 * x633) + (-1 * x635 * x580) + (-1 * x599) + (-1 * x54 * x631) + (-1 * x52 * x634) + (-1 * x50 * x630) + (-1 * x84 * x622) + x601 + (x88 * x623) + x600;
	const FLT x640 = x88 * x637;
	const FLT x641 = x84 * x636;
	const FLT x642 = (-1 * x640) + x584 + (-1 * x55 * x631) + (-1 * x558) + (x52 * x630) + x85 + (x87 * x622) + (-1 * x623 * x590) + (-1 * x54 * x633) + (-1 * x50 * x634) + (-1 * x635 * x586) + x641;
	const FLT x643 = x637 * x580;
	const FLT x644 = x94 * x636;
	const FLT x645 = -1 * x81;
	const FLT x646 = (-1 * x635 * x590) + (x440 * x632) + x645 + (x437 * x629) + (x623 * x586) + (-1 * x436 * x629) + (x55 * x630) + x595 + (-1 * x91 * x622) + x643 + (-1 * x644);
	const FLT x647 = (x637 * x586) + (-1 * x91 * x636);
	const FLT x648 = x647 + (-1 * x446 * x629) + x607 + x82 + (-1 * x54 * x630) + (-1 * x88 * x635) + (-1 * x52 * x633) + (x94 * x622) + (-1 * x623 * x580) + (x447 * x629);
	const FLT x649 = (x648 * sensor_pt[1]) + (-1 * x642 * sensor_pt[0]) + (x646 * sensor_pt[2]);
	const FLT x650 = (x639 * sensor_pt[0]) + (-1 * x648 * sensor_pt[2]) + (x646 * sensor_pt[1]);
	const FLT x651 = (-1 * x454 * x650) + (-1 * x426 * x639) + (x455 * x642) + (x452 * x649);
	const FLT x652 = (x642 * sensor_pt[2]) + (-1 * x639 * sensor_pt[1]) + (x646 * sensor_pt[0]);
	const FLT x653 = (x454 * x652) + (-1 * x455 * x648) + (-1 * x458 * x649) + (x459 * x639);
	const FLT x654 = (x458 * x650) + (-1 * x452 * x652) + (-1 * x459 * x642) + (x426 * x648);
	const FLT x655 = (-1 * x43 * x654) + (x457 * x651) + (x41 * x653);
	const FLT x656 = (-1 * x41 * x651) + (x654 * x108) + (x457 * x653);
	const FLT x657 = x654 + (x655 * x218) + (-1 * x656 * x201);
	const FLT x658 = (-1 * x653 * x108) + (x43 * x651) + (x457 * x654);
	const FLT x659 = x651 + (x656 * x230) + (-1 * x658 * x218);
	const FLT x660 = x653 + (-1 * x655 * x230) + (x658 * x201);
	const FLT x661 = (-1 * x136 * ((-1 * ((x657 * x140) + (x660 * x139)) * x141) + (x659 * x138))) + (-1 * ((x660 * x119) + (-1 * x657 * x125)) * x134);
	const FLT x662 = x59 * x582;
	const FLT x663 = x59 * x553;
	const FLT x664 = x65 * x46;
	const FLT x665 = x46 * x46 * x46;
	const FLT x666 = ((x664 * x567) + (x665 * x567) + (-1 * x665 * x563) + (-1 * x46 * x624) + (-1 * x664 * x563) + (x46 * x625) + (-1 * x46 * x573) + (x46 * x571)) * x574;
	const FLT x667 = x413 * x666;
	const FLT x668 = x440 * x666;
	const FLT x669 = x412 * x666;
	const FLT x670 = x64 * x669;
	const FLT x671 = x644 + (-1 * x52 * x670) + (x46 * x668) + (-1 * x54 * x667) + (-1 * x643) + (-1 * x446 * x666) + (x662 * x586) + x645 + x597 + (-1 * x91 * x663) + (-1 * x46 * x579);
	const FLT x672 = x46 * x669;
	const FLT x673 = x46 * x578;
	const FLT x674 = (-1 * x54 * x672) + (-1 * x416 * x666) + (-1 * x50 * x667) + (-1 * x673 * x580) + x85 + (-1 * x662 * x590) + (-1 * x589) + x556 + x640 + (x87 * x663) + (x55 * x670) + (-1 * x641);
	const FLT x675 = (-1 * x673 * x590) + (x662 * x580) + (x55 * x667) + (-1 * x606) + x647 + x604 + (-1 * x436 * x666) + (-1 * x94 * x663) + (x52 * x672) + (x64 * x668) + x605;
	const FLT x676 = x638 + (-1 * x88 * x662) + (-1 * x673 * x586) + x602 + (x52 * x667) + (-1 * x54 * x670) + x89 + (-1 * x55 * x672) + (-1 * x427 * x666) + (x84 * x663);
	const FLT x677 = (x671 * sensor_pt[1]) + (x675 * sensor_pt[2]) + (-1 * x676 * sensor_pt[0]);
	const FLT x678 = (x676 * sensor_pt[2]) + (-1 * x674 * sensor_pt[1]) + (x675 * sensor_pt[0]);
	const FLT x679 = (-1 * x455 * x671) + (x454 * x678) + (-1 * x458 * x677) + (x459 * x674);
	const FLT x680 = (x674 * sensor_pt[0]) + (-1 * x671 * sensor_pt[2]) + (x675 * sensor_pt[1]);
	const FLT x681 = (x452 * x677) + (-1 * x426 * x674) + (x455 * x676) + (-1 * x454 * x680);
	const FLT x682 = (-1 * x452 * x678) + (x458 * x680) + (-1 * x459 * x676) + (x426 * x671);
	const FLT x683 = (-1 * x43 * x682) + (x41 * x679) + (x457 * x681);
	const FLT x684 = (-1 * x41 * x681) + (x682 * x108) + (x457 * x679);
	const FLT x685 = x682 + (x683 * x218) + (-1 * x684 * x201);
	const FLT x686 = (-1 * x679 * x108) + (x43 * x681) + (x457 * x682);
	const FLT x687 = x681 + (x684 * x230) + (-1 * x686 * x218);
	const FLT x688 = x679 + (x686 * x201) + (-1 * x683 * x230);
	const FLT x689 = (-1 * x136 * ((-1 * ((x685 * x140) + (x688 * x139)) * x141) + (x687 * x138))) + (-1 * ((x688 * x119) + (-1 * x685 * x125)) * x134);
	const FLT x690 = dt * x28;
	const FLT x691 = dt * x23;
	const FLT x692 = x691 + x690;
	const FLT x693 = -1 * dt * x115;
	const FLT x694 = -1 * dt * x116;
	const FLT x695 = x694 + x693 + dt;
	const FLT x696 = dt * x129;
	const FLT x697 = dt * x128;
	const FLT x698 = (-1 * x697) + x696;
	const FLT x699 = (-1 * x136 * ((-1 * ((x692 * x140) + (x698 * x139)) * x141) + (x695 * x138))) + (-1 * ((x698 * x119) + (-1 * x692 * x125)) * x134);
	const FLT x700 = dt * x146;
	const FLT x701 = dt * x145;
	const FLT x702 = (-1 * x701) + x700;
	const FLT x703 = x696 + x697;
	const FLT x704 = (-1 * dt * x151) + dt;
	const FLT x705 = x704 + x694;
	const FLT x706 = (-1 * x136 * ((-1 * ((x702 * x140) + (x705 * x139)) * x141) + (x703 * x138))) + (-1 * ((x705 * x119) + (-1 * x702 * x125)) * x134);
	const FLT x707 = x704 + x693;
	const FLT x708 = (-1 * x690) + x691;
	const FLT x709 = x700 + x701;
	const FLT x710 = (-1 * x136 * ((-1 * ((x707 * x140) + (x709 * x139)) * x141) + (x708 * x138))) + (-1 * ((x709 * x119) + (-1 * x707 * x125)) * x134);
	const FLT x711 = x106 * x137 * x136;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), (x123 * ((-1 * x118 * x119) + (x30 * x113))) + x142 + (x142 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), (((-1 * x119 * x150) + (x113 * x148)) * x123) + x153 + (x144 * x153));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), x158 + (((-1 * x119 * x155) + (x113 * x154)) * x123) + (x144 * x158));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x255 + (((-1 * x248 * x119) + (x229 * x113)) * x123) + (x255 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x304 + (((-1 * x300 * x119) + (x292 * x113)) * x123) + (x304 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), (((-1 * x331 * x119) + (x325 * x113)) * x123) + x334 + (x334 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x361 + (((-1 * x359 * x119) + (x355 * x113)) * x123) + (x361 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), x375 + (((-1 * x370 * x119) + (x367 * x113)) * x123) + (x375 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), x382 + (((-1 * x379 * x119) + (x378 * x113)) * x123) + (x382 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), x386 + (((-1 * x384 * x119) + (x383 * x113)) * x123) + (x386 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), (((-1 * x390 * x119) + (x387 * x113)) * x123) + x392 + (x392 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), x397 + (((-1 * x394 * x119) + (x393 * x113)) * x123) + (x397 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), x401 + (((-1 * x399 * x119) + (x398 * x113)) * x123) + (x401 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x469 + (((-1 * x467 * x119) + (x465 * x113)) * x123) + (x469 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x502 + (((-1 * x500 * x119) + (x498 * x113)) * x123) + (x502 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), (((-1 * x526 * x119) + (x524 * x113)) * x123) + x528 + (x528 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x551 + (((-1 * x549 * x119) + (x547 * x113)) * x123) + (x551 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x621 + (((-1 * x619 * x119) + (x617 * x113)) * x123) + (x621 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x661 + (((-1 * x659 * x119) + (x657 * x113)) * x123) + (x661 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x689 + (((-1 * x687 * x119) + (x685 * x113)) * x123) + (x689 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), x699 + (((-1 * x695 * x119) + (x692 * x113)) * x123) + (x699 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), x706 + (((-1 * x703 * x119) + (x702 * x113)) * x123) + (x706 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), x710 + (((-1 * x708 * x119) + (x707 * x113)) * x123) + (x710 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), x121 * x121);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), -1 * cos(x143));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), x144);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), -1 + (-1 * x144));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), (-1 * x711 * x144) + (-1 * x711));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]

static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_x0(Hx, dt, _x0, error_model, sensor_pt);
    }
}
// Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_error_model(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x3 = (*_x0).Lighthouse.Rot[1] + (x2 * (*_x0).Lighthouse.Rot[0]) + (-1 * x0 * (*_x0).Lighthouse.Rot[3]) + (x1 * (*_x0).Lighthouse.Rot[2]);
	const FLT x4 = 1.0 * x3;
	const FLT x5 = x4 * (*_x0).Lighthouse.Rot[0];
	const FLT x6 = (-1 * x1 * (*_x0).Lighthouse.Rot[1]) + (x0 * (*_x0).Lighthouse.Rot[0]) + (x2 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2];
	const FLT x7 = 1.0 * (*_x0).Lighthouse.Rot[3];
	const FLT x8 = x6 * x7;
	const FLT x9 = (x0 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x10 = 1.0 * (*_x0).Lighthouse.Rot[2];
	const FLT x11 = x9 * x10;
	const FLT x12 = (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (-1 * x0 * (*_x0).Lighthouse.Rot[2]);
	const FLT x13 = 1.0 * x12;
	const FLT x14 = x13 * (*_x0).Lighthouse.Rot[1];
	const FLT x15 = (-1 * x11) + (-1 * x14) + x5 + x8;
	const FLT x16 = x9 * x9;
	const FLT x17 = x12 * x12;
	const FLT x18 = x6 * x6;
	const FLT x19 = x3 * x3;
	const FLT x20 = x19 + x18 + x16 + x17;
	const FLT x21 = 1. / x20;
	const FLT x22 = x21 * x16;
	const FLT x23 = x21 * x18;
	const FLT x24 = x21 * x19;
	const FLT x25 = x24 + (x21 * x17) + x22 + x23;
	const FLT x26 = 1. / sqrt(x25);
	const FLT x27 = (1. / (x20 * sqrt(x20))) * x26;
	const FLT x28 = x27 * x15;
	const FLT x29 = dt * fabs(dt);
	const FLT x30 = 1.0/2.0 * x29;
	const FLT x31 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x32 = dt * dt;
	const FLT x33 = x31 * x31;
	const FLT x34 = x32 * x33;
	const FLT x35 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x36 = x35 * x35;
	const FLT x37 = x32 * x36;
	const FLT x38 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x39 = x38 * x38;
	const FLT x40 = x32 * x39;
	const FLT x41 = 1e-10 + x40 + x34 + x37;
	const FLT x42 = sqrt(x41);
	const FLT x43 = 0.5 * x42;
	const FLT x44 = sin(x43);
	const FLT x45 = (1. / x42) * x44;
	const FLT x46 = x45 * x31;
	const FLT x47 = dt * x46;
	const FLT x48 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x49 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x50 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x51 = (-1 * x49 * (*_x0).Object.Pose.Rot[2]) + (x50 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[3] + (x48 * (*_x0).Object.Pose.Rot[0]);
	const FLT x52 = (-1 * x48 * (*_x0).Object.Pose.Rot[1]) + (x50 * (*_x0).Object.Pose.Rot[0]) + (x49 * (*_x0).Object.Pose.Rot[3]) + (*_x0).Object.Pose.Rot[2];
	const FLT x53 = (-1 * x49 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[0] + (-1 * x48 * (*_x0).Object.Pose.Rot[3]) + (-1 * x50 * (*_x0).Object.Pose.Rot[2]);
	const FLT x54 = (*_x0).Object.Pose.Rot[1] + (x49 * (*_x0).Object.Pose.Rot[0]) + (-1 * x50 * (*_x0).Object.Pose.Rot[3]) + (x48 * (*_x0).Object.Pose.Rot[2]);
	const FLT x55 = (x54 * x54) + (x51 * x51) + (x53 * x53) + (x52 * x52);
	const FLT x56 = 1. / sqrt(x55);
	const FLT x57 = x44 * x44;
	const FLT x58 = 1. / x41;
	const FLT x59 = x58 * x57;
	const FLT x60 = cos(x43);
	const FLT x61 = (x59 * x37) + (x60 * x60) + (x59 * x34) + (x59 * x40);
	const FLT x62 = 1. / sqrt(x61);
	const FLT x63 = x62 * x53;
	const FLT x64 = x63 * x56;
	const FLT x65 = x62 * x54;
	const FLT x66 = dt * x45;
	const FLT x67 = x66 * x56;
	const FLT x68 = x67 * x65;
	const FLT x69 = x60 * x56;
	const FLT x70 = x62 * x51;
	const FLT x71 = x70 * x69;
	const FLT x72 = x62 * x52;
	const FLT x73 = x72 * x67;
	const FLT x74 = (x73 * x35) + x71 + (x64 * x47) + (-1 * x68 * x38);
	const FLT x75 = x70 * x56;
	const FLT x76 = x62 * x69;
	const FLT x77 = x76 * x53;
	const FLT x78 = x77 + (-1 * x68 * x35) + (-1 * x75 * x47) + (-1 * x73 * x38);
	const FLT x79 = x72 * x56;
	const FLT x80 = x75 * x66;
	const FLT x81 = x76 * x54;
	const FLT x82 = x63 * x67;
	const FLT x83 = x81 + (-1 * x79 * x47) + (x82 * x35) + (x80 * x38);
	const FLT x84 = (-1 * x83 * sensor_pt[2]) + (x78 * sensor_pt[1]) + (x74 * sensor_pt[0]);
	const FLT x85 = x76 * x52;
	const FLT x86 = x65 * x56;
	const FLT x87 = (-1 * x80 * x35) + (x82 * x38) + (x86 * x47) + x85;
	const FLT x88 = (-1 * x87 * sensor_pt[0]) + (x78 * sensor_pt[2]) + (x83 * sensor_pt[1]);
	const FLT x89 = (*_x0).Object.Pose.Pos[0] + (x30 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (2 * ((x88 * x87) + (-1 * x84 * x74))) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0]));
	const FLT x90 = 1. / sqrt(x20);
	const FLT x91 = x90 * x26;
	const FLT x92 = x91 * x12;
	const FLT x93 = (-1 * x74 * sensor_pt[1]) + (x78 * sensor_pt[0]) + (x87 * sensor_pt[2]);
	const FLT x94 = (2 * ((x83 * x84) + (-1 * x87 * x93))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (*_x0).Object.Pose.Pos[2] + (x30 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x95 = x6 * x91;
	const FLT x96 = (*_x0).Object.Pose.Pos[1] + (*error_model).Object.Pose.Pos[1] + (x30 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (2 * ((x74 * x93) + (-1 * x83 * x88))) + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1]));
	const FLT x97 = x9 * x91;
	const FLT x98 = (x97 * x96) + (x89 * x92) + (-1 * x95 * x94);
	const FLT x99 = x6 * x98;
	const FLT x100 = 1.0/2.0 * x96;
	const FLT x101 = x9 * x28;
	const FLT x102 = 1.0/2.0 * x89;
	const FLT x103 = x28 * x102;
	const FLT x104 = 1.0/2.0 * x94;
	const FLT x105 = x28 * x104;
	const FLT x106 = x90 * (1. / (x25 * sqrt(x25)));
	const FLT x107 = x12 * x106;
	const FLT x108 = 1. / (x20 * x20);
	const FLT x109 = x17 * x108;
	const FLT x110 = x15 * x108;
	const FLT x111 = (-1 * x21 * x14) + (-1 * x18 * x110) + (x5 * x21) + (-1 * x15 * x109) + (-1 * x21 * x11) + (x8 * x21) + (-1 * x16 * x110) + (-1 * x19 * x110);
	const FLT x112 = x102 * x111;
	const FLT x113 = x9 * x106;
	const FLT x114 = x100 * x111;
	const FLT x115 = x6 * x106;
	const FLT x116 = x104 * x111;
	const FLT x117 = 0.5 * x91;
	const FLT x118 = x96 * x117;
	const FLT x119 = x89 * x117;
	const FLT x120 = x94 * x117;
	const FLT x121 = (-1 * x120 * (*_x0).Lighthouse.Rot[3]) + (-1 * x118 * (*_x0).Lighthouse.Rot[2]) + (-1 * x119 * (*_x0).Lighthouse.Rot[1]);
	const FLT x122 = x121 + (x115 * x116) + (-1 * x12 * x103) + (-1 * x107 * x112) + (x6 * x105) + (-1 * x101 * x100) + (-1 * x113 * x114);
	const FLT x123 = 2 * x95;
	const FLT x124 = x3 * x91;
	const FLT x125 = (x92 * x96) + (x94 * x124) + (-1 * x89 * x97);
	const FLT x126 = x3 * x125;
	const FLT x127 = x3 * x106;
	const FLT x128 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x129 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x130 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x131 = (x124 * x130) + (x92 * x128) + (-1 * x97 * x129);
	const FLT x132 = x111 * x131;
	const FLT x133 = x28 * x131;
	const FLT x134 = x111 * x125;
	const FLT x135 = 1.0/2.0 * x111;
	const FLT x136 = x128 * x135;
	const FLT x137 = 1.0/2.0 * x129;
	const FLT x138 = x28 * x12;
	const FLT x139 = x130 * x135;
	const FLT x140 = 1.0/2.0 * x128;
	const FLT x141 = x28 * x140;
	const FLT x142 = 1.0/2.0 * x130;
	const FLT x143 = x6 * x28;
	const FLT x144 = x129 * x135;
	const FLT x145 = x117 * x129;
	const FLT x146 = x117 * (*_x0).Lighthouse.Rot[2];
	const FLT x147 = x117 * (*_x0).Lighthouse.Rot[3];
	const FLT x148 = (-1 * x130 * x147) + (-1 * x145 * (*_x0).Lighthouse.Rot[1]) + (-1 * x128 * x146);
	const FLT x149 = (-1 * x107 * x144) + (-1 * x137 * x138) + (x115 * x139) + (x142 * x143) + x148 + (-1 * x113 * x136) + (-1 * x9 * x141);
	const FLT x150 = (x97 * x128) + (-1 * x95 * x130) + (x92 * x129);
	const FLT x151 = x111 * x150;
	const FLT x152 = x28 * x150;
	const FLT x153 = x91 * x98;
	const FLT x154 = 1.0 * (*_x0).Lighthouse.Rot[0];
	const FLT x155 = x91 * x125;
	const FLT x156 = x154 * x155;
	const FLT x157 = x98 * x111;
	const FLT x158 = x91 * x131;
	const FLT x159 = x154 * x158;
	const FLT x160 = x120 * (*_x0).Lighthouse.Rot[0];
	const FLT x161 = x118 * (*_x0).Lighthouse.Rot[1];
	const FLT x162 = x119 * (*_x0).Lighthouse.Rot[2];
	const FLT x163 = (x112 * x113) + x162 + (-1 * x107 * x114) + (x9 * x103) + (-1 * x116 * x127) + x160 + (-1 * x3 * x105) + (-1 * x161) + (-1 * x100 * x138);
	const FLT x164 = 2 * x124;
	const FLT x165 = x3 * x142;
	const FLT x166 = x117 * (*_x0).Lighthouse.Rot[0];
	const FLT x167 = x166 * x130;
	const FLT x168 = x127 * x135;
	const FLT x169 = x117 * (*_x0).Lighthouse.Rot[1];
	const FLT x170 = x128 * x169;
	const FLT x171 = x145 * (*_x0).Lighthouse.Rot[2];
	const FLT x172 = x171 + (x113 * x144) + (-1 * x28 * x165) + (-1 * x12 * x141) + (x101 * x137) + (-1 * x170) + x167 + (-1 * x107 * x136) + (-1 * x168 * x130);
	const FLT x173 = x91 * x150;
	const FLT x174 = (-1 * x7 * x173) + (x164 * x172) + (-1 * x164 * x163) + (-1 * x115 * x157) + (-1 * x3 * x133) + (-1 * x123 * x149) + (x28 * x126) + (-1 * x99 * x28) + (x123 * x122) + (x127 * x134) + x159 + (x7 * x153) + (-1 * x127 * x132) + (x115 * x151) + (x6 * x152) + (-1 * x156);
	const FLT x175 = (x95 * x129) + (x92 * x130) + (-1 * x124 * x128);
	const FLT x176 = (x89 * x95) + (x92 * x94) + (-1 * x96 * x124);
	const FLT x177 = x89 + (-1 * (x129 + (2 * ((-1 * x95 * x175) + (x97 * x131))))) + (2 * ((-1 * x95 * x176) + (x97 * x125)));
	const FLT x178 = x94 + (-1 * (x130 + (2 * ((-1 * x124 * x131) + (x95 * x150))))) + (2 * ((-1 * x124 * x125) + (x98 * x95)));
	const FLT x179 = x178 * x178;
	const FLT x180 = 1. / x179;
	const FLT x181 = x177 * x180;
	const FLT x182 = x115 * x176;
	const FLT x183 = x119 * (*_x0).Lighthouse.Rot[3];
	const FLT x184 = x120 * (*_x0).Lighthouse.Rot[1];
	const FLT x185 = x118 * (*_x0).Lighthouse.Rot[0];
	const FLT x186 = (-1 * x112 * x115) + (-1 * x185) + (x114 * x127) + x183 + (-1 * x107 * x116) + (-1 * x102 * x143) + (-1 * x12 * x105) + (x3 * x28 * x100) + (-1 * x184);
	const FLT x187 = x9 * x125;
	const FLT x188 = x28 * x175;
	const FLT x189 = x111 * x175;
	const FLT x190 = x6 * x176;
	const FLT x191 = 2 * x97;
	const FLT x192 = x145 * (*_x0).Lighthouse.Rot[3];
	const FLT x193 = x169 * x130;
	const FLT x194 = x128 * x166;
	const FLT x195 = (x128 * x168) + (-1 * x137 * x143) + (-1 * x194) + x192 + (-1 * x115 * x144) + (-1 * x107 * x139) + (x3 * x141) + (-1 * x193) + (-1 * x138 * x142);
	const FLT x196 = (x10 * x158) + (-1 * x10 * x155);
	const FLT x197 = x91 * x176;
	const FLT x198 = x91 * x175;
	const FLT x199 = (x7 * x198) + (-1 * x7 * x197);
	const FLT x200 = x199 + (-1 * x113 * x134) + (-1 * x115 * x189) + (-1 * x6 * x188) + (x9 * x133) + (x111 * x182) + x196 + (x28 * x190) + (-1 * x28 * x187) + (x113 * x132) + (x163 * x191) + (-1 * x123 * x186) + (-1 * x172 * x191) + (x123 * x195);
	const FLT x201 = 1. / x178;
	const FLT x202 = -1 * x178;
	const FLT x203 = atan2(x177, x202);
	const FLT x204 = x177 * x177;
	const FLT x205 = 2 * (1. / (x179 + x204)) * x203 * x179 * ((*error_model).BSD1.curve + (*_x0).BSD1.curve);
	const FLT x206 = x96 + (2 * ((-1 * x98 * x97) + (x124 * x176))) + (-1 * (x128 + (2 * ((-1 * x97 * x150) + (x124 * x175)))));
	const FLT x207 = x206 * x180;
	const FLT x208 = x197 * x154;
	const FLT x209 = x3 * x176;
	const FLT x210 = x9 * x98;
	const FLT x211 = x198 * x154;
	const FLT x212 = x209 * x106;
	const FLT x213 = (-1 * x212 * x111) + x208 + (x164 * x186) + (x28 * x210) + (x10 * x153) + (-1 * x113 * x151) + (x191 * x149) + (x113 * x157) + (-1 * x211) + (-1 * x164 * x195) + (x3 * x188) + (-1 * x122 * x191) + (x127 * x189) + (-1 * x10 * x173) + (-1 * x28 * x209) + (-1 * x9 * x152);
	const FLT x214 = x179 + (x206 * x206);
	const FLT x215 = 1. / x214;
	const FLT x216 = x215 * x179;
	const FLT x217 = 1. / sqrt(x214);
	const FLT x218 = (*error_model).BSD1.tilt + (*_x0).BSD1.tilt;
	const FLT x219 = x218 * x217;
	const FLT x220 = 2 * x206;
	const FLT x221 = 2 * x178;
	const FLT x222 = 1.0/2.0 * x218 * (1. / (x214 * sqrt(x214))) * x177;
	const FLT x223 = 1. / sqrt(1 + (-1 * (x218 * x218) * x215 * x204));
	const FLT x224 = (-1 * x223 * ((-1 * ((x221 * x174) + (x213 * x220)) * x222) + (x219 * x200))) + (-1 * ((x213 * x201) + (-1 * x207 * x174)) * x216);
	const FLT x225 = 1.5707963267949 + (*error_model).BSD1.gibpha + (-1 * atan2(-1 * x206, x202)) + (-1 * ((*error_model).BSD1.phase + (*_x0).BSD1.phase)) + (*_x0).BSD1.gibpha + (-1 * asin(x219 * x177));
	const FLT x226 = sin(x225) * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag);
	const FLT x227 = x3 * x7;
	const FLT x228 = x6 * x154;
	const FLT x229 = 1.0 * (*_x0).Lighthouse.Rot[1];
	const FLT x230 = x9 * x229;
	const FLT x231 = x13 * (*_x0).Lighthouse.Rot[2];
	const FLT x232 = (-1 * x231) + x230 + (-1 * x227) + x228;
	const FLT x233 = x232 * x108;
	const FLT x234 = x106 * ((-1 * x18 * x233) + (x21 * x228) + (x21 * x230) + (-1 * x19 * x233) + (-1 * x21 * x227) + (-1 * x16 * x233) + (-1 * x21 * x231) + (-1 * x232 * x109));
	const FLT x235 = x6 * x234;
	const FLT x236 = x153 * x154;
	const FLT x237 = x27 * x232;
	const FLT x238 = x237 * x100;
	const FLT x239 = x9 * x234;
	const FLT x240 = x6 * x237;
	const FLT x241 = x12 * x237;
	const FLT x242 = x12 * x234;
	const FLT x243 = (-1 * x162) + (x235 * x104) + (-1 * x160) + (-1 * x242 * x102) + (-1 * x9 * x238) + x161 + (x240 * x104) + (-1 * x239 * x100) + (-1 * x241 * x102);
	const FLT x244 = x237 * x131;
	const FLT x245 = x9 * x237;
	const FLT x246 = x3 * x237;
	const FLT x247 = x3 * x234;
	const FLT x248 = x121 + (x239 * x102) + (-1 * x242 * x100) + (-1 * x247 * x104) + (x245 * x102) + (-1 * x246 * x104) + (-1 * x241 * x100);
	const FLT x249 = x237 * x150;
	const FLT x250 = x173 * x154;
	const FLT x251 = x148 + (x239 * x137) + (-1 * x247 * x142) + (-1 * x241 * x140) + (x245 * x137) + (-1 * x237 * x165) + (-1 * x242 * x140);
	const FLT x252 = (-1 * x242 * x137) + (x240 * x142) + (-1 * x167) + x170 + (-1 * x171) + (-1 * x245 * x140) + (x235 * x142) + (-1 * x241 * x137) + (-1 * x239 * x140);
	const FLT x253 = (-1 * x252 * x123) + (-1 * x99 * x237) + (-1 * x3 * x244) + (x243 * x123) + (x234 * x126) + (x237 * x126) + (-1 * x7 * x158) + (x7 * x155) + (-1 * x98 * x235) + x236 + (-1 * x248 * x164) + (x6 * x249) + (-1 * x250) + (x235 * x150) + (x251 * x164) + (-1 * x247 * x131);
	const FLT x254 = x130 * x146;
	const FLT x255 = x128 * x147;
	const FLT x256 = x145 * (*_x0).Lighthouse.Rot[0];
	const FLT x257 = x256 + (-1 * x235 * x137) + (x247 * x140) + x255 + (-1 * x240 * x137) + (x246 * x140) + (-1 * x241 * x142) + (-1 * x242 * x142) + (-1 * x254);
	const FLT x258 = x119 * (*_x0).Lighthouse.Rot[0];
	const FLT x259 = x120 * (*_x0).Lighthouse.Rot[2];
	const FLT x260 = x118 * (*_x0).Lighthouse.Rot[3];
	const FLT x261 = (x247 * x100) + x260 + (-1 * x241 * x104) + (-1 * x259) + x258 + (-1 * x242 * x104) + (-1 * x240 * x102) + (-1 * x235 * x102) + (x3 * x238);
	const FLT x262 = x237 * x175;
	const FLT x263 = (x235 * x176) + (x239 * x131) + (-1 * x229 * x158) + (-1 * x234 * x187) + (-1 * x261 * x123) + (x229 * x155) + (-1 * x6 * x262) + (x248 * x191) + (-1 * x235 * x175) + x211 + (-1 * x251 * x191) + (x257 * x123) + (x9 * x244) + (-1 * x237 * x187) + (x237 * x190) + (-1 * x208);
	const FLT x264 = (x229 * x173) + (-1 * x229 * x153);
	const FLT x265 = x199 + (x247 * x175) + (-1 * x243 * x191) + (x3 * x262) + (-1 * x9 * x249) + (x252 * x191) + (-1 * x239 * x150) + (x261 * x164) + (-1 * x237 * x209) + (x210 * x234) + (x210 * x237) + (-1 * x234 * x209) + x264 + (-1 * x257 * x164);
	const FLT x266 = (-1 * x223 * ((-1 * ((x253 * x221) + (x265 * x220)) * x222) + (x219 * x263))) + (-1 * ((x201 * x265) + (-1 * x207 * x253)) * x216);
	const FLT x267 = x4 * (*_x0).Lighthouse.Rot[2];
	const FLT x268 = x6 * x229;
	const FLT x269 = x9 * x154;
	const FLT x270 = x13 * (*_x0).Lighthouse.Rot[3];
	const FLT x271 = x269 + (-1 * x270) + x267 + (-1 * x268);
	const FLT x272 = x271 * x108;
	const FLT x273 = (-1 * x17 * x272) + (-1 * x21 * x268) + (x21 * x267) + (x21 * x269) + (-1 * x19 * x272) + (-1 * x21 * x270) + (-1 * x18 * x272) + (-1 * x16 * x272);
	const FLT x274 = x98 * x273;
	const FLT x275 = x273 * x100;
	const FLT x276 = x27 * x271;
	const FLT x277 = x276 * x104;
	const FLT x278 = x273 * x102;
	const FLT x279 = x276 * x100;
	const FLT x280 = x276 * x102;
	const FLT x281 = x273 * x115;
	const FLT x282 = x184 + (x281 * x104) + (-1 * x183) + x185 + (-1 * x275 * x113) + (-1 * x12 * x280) + (x6 * x277) + (-1 * x278 * x107) + (-1 * x9 * x279);
	const FLT x283 = x276 * x150;
	const FLT x284 = x273 * x107;
	const FLT x285 = x276 * x140;
	const FLT x286 = x273 * x113;
	const FLT x287 = x12 * x276;
	const FLT x288 = x6 * x276;
	const FLT x289 = (-1 * x287 * x137) + (-1 * x286 * x140) + x193 + (x288 * x142) + (x281 * x142) + (-1 * x192) + x194 + (-1 * x284 * x137) + (-1 * x9 * x285);
	const FLT x290 = x273 * x125;
	const FLT x291 = x273 * x127;
	const FLT x292 = (x286 * x137) + (-1 * x284 * x140) + x254 + (-1 * x291 * x142) + (-1 * x12 * x285) + (-1 * x276 * x165) + (-1 * x255) + (-1 * x256) + (x9 * x276 * x137);
	const FLT x293 = x276 * x131;
	const FLT x294 = x273 * x150;
	const FLT x295 = x273 * x131;
	const FLT x296 = x273 * x104;
	const FLT x297 = (-1 * x258) + (-1 * x275 * x107) + (-1 * x260) + (x9 * x280) + (-1 * x296 * x127) + (-1 * x3 * x277) + (-1 * x12 * x279) + (x278 * x113) + x259;
	const FLT x298 = x264 + x196 + (-1 * x297 * x164) + (x290 * x127) + (x6 * x283) + (-1 * x289 * x123) + (-1 * x274 * x115) + (x282 * x123) + (x292 * x164) + (x276 * x126) + (-1 * x99 * x276) + (x294 * x115) + (-1 * x3 * x293) + (-1 * x295 * x127);
	const FLT x299 = x121 + (x275 * x127) + (x3 * x279) + (-1 * x281 * x102) + (-1 * x296 * x107) + (-1 * x12 * x277) + (-1 * x6 * x280);
	const FLT x300 = x148 + (-1 * x287 * x142) + (x291 * x140) + (-1 * x288 * x137) + (-1 * x281 * x137) + (x3 * x285) + (-1 * x284 * x142);
	const FLT x301 = x273 * x175;
	const FLT x302 = x276 * x175;
	const FLT x303 = (-1 * x159) + (-1 * x299 * x123) + x156 + (x276 * x190) + (-1 * x276 * x187) + (x229 * x197) + (x297 * x191) + (x295 * x113) + (-1 * x229 * x198) + (x9 * x293) + (-1 * x292 * x191) + (x300 * x123) + (x273 * x182) + (-1 * x290 * x113) + (-1 * x6 * x302) + (-1 * x301 * x115);
	const FLT x304 = (-1 * x209 * x276) + (x274 * x113) + (-1 * x282 * x191) + (-1 * x10 * x198) + (x210 * x276) + x250 + (-1 * x9 * x283) + (x10 * x197) + (x3 * x302) + (x299 * x164) + (-1 * x300 * x164) + (-1 * x236) + (x301 * x127) + (x289 * x191) + (-1 * x294 * x113) + (-1 * x212 * x273);
	const FLT x305 = (-1 * x223 * ((-1 * ((x298 * x221) + (x220 * x304)) * x222) + (x219 * x303))) + (-1 * ((x201 * x304) + (-1 * x207 * x298)) * x216);
	const FLT x306 = 1. / x25;
	const FLT x307 = 2 * x306;
	const FLT x308 = x21 * x307;
	const FLT x309 = x3 * x9;
	const FLT x310 = x309 * x308;
	const FLT x311 = -1 * x310;
	const FLT x312 = x12 * x308;
	const FLT x313 = x6 * x312;
	const FLT x314 = -1 * x313;
	const FLT x315 = x314 + x311;
	const FLT x316 = x23 * x307;
	const FLT x317 = x22 * x307;
	const FLT x318 = -1 + x317 + x316;
	const FLT x319 = x9 * x312;
	const FLT x320 = x6 * x308;
	const FLT x321 = x3 * x320;
	const FLT x322 = -1 * x321;
	const FLT x323 = x322 + x319;
	const FLT x324 = (-1 * x223 * ((-1 * ((x221 * x315) + (x220 * x323)) * x222) + (x219 * x318))) + (-1 * ((x201 * x323) + (-1 * x207 * x315)) * x216);
	const FLT x325 = x3 * x312;
	const FLT x326 = x9 * x320;
	const FLT x327 = -1 * x326;
	const FLT x328 = x327 + x325;
	const FLT x329 = -1 * x319;
	const FLT x330 = x329 + x322;
	const FLT x331 = x24 * x307;
	const FLT x332 = -1 + x331;
	const FLT x333 = x332 + x317;
	const FLT x334 = (-1 * x223 * ((-1 * ((x221 * x328) + (x220 * x333)) * x222) + (x219 * x330))) + (-1 * ((x201 * x333) + (-1 * x207 * x328)) * x216);
	const FLT x335 = x332 + x316;
	const FLT x336 = x311 + x313;
	const FLT x337 = -1 * x325;
	const FLT x338 = x337 + x327;
	const FLT x339 = (-1 * x223 * ((-1 * ((x221 * x335) + (x220 * x338)) * x222) + (x219 * x336))) + (-1 * ((x201 * x338) + (-1 * x207 * x335)) * x216);
	const FLT x340 = x29 * x306;
	const FLT x341 = x21 * x340;
	const FLT x342 = x12 * x341;
	const FLT x343 = x6 * x342;
	const FLT x344 = x309 * x341;
	const FLT x345 = x344 + x343;
	const FLT x346 = -1 * x23 * x340;
	const FLT x347 = -1 * x22 * x340;
	const FLT x348 = x347 + x30 + x346;
	const FLT x349 = x6 * x341;
	const FLT x350 = x3 * x349;
	const FLT x351 = x9 * x342;
	const FLT x352 = (-1 * x351) + x350;
	const FLT x353 = (-1 * x223 * ((-1 * ((x221 * x345) + (x220 * x352)) * x222) + (x219 * x348))) + (-1 * ((x201 * x352) + (-1 * x207 * x345)) * x216);
	const FLT x354 = x9 * x349;
	const FLT x355 = x3 * x342;
	const FLT x356 = (-1 * x355) + x354;
	const FLT x357 = x350 + x351;
	const FLT x358 = (-1 * x24 * x340) + x30;
	const FLT x359 = x358 + x347;
	const FLT x360 = (-1 * x223 * ((-1 * ((x221 * x356) + (x220 * x359)) * x222) + (x219 * x357))) + (-1 * ((x201 * x359) + (-1 * x207 * x356)) * x216);
	const FLT x361 = x358 + x346;
	const FLT x362 = (-1 * x343) + x344;
	const FLT x363 = x354 + x355;
	const FLT x364 = (-1 * x223 * ((-1 * ((x221 * x361) + (x220 * x363)) * x222) + (x219 * x362))) + (-1 * ((x201 * x363) + (-1 * x207 * x361)) * x216);
	const FLT x365 = 1.0 * x54;
	const FLT x366 = 1.0 * (*_x0).Object.Pose.Rot[1];
	const FLT x367 = 1.0 * x51;
	const FLT x368 = 1.0 * x52;
	const FLT x369 = (x368 * (*_x0).Object.Pose.Rot[3]) + (-1 * x367 * (*_x0).Object.Pose.Rot[2]) + (x365 * (*_x0).Object.Pose.Rot[0]) + (-1 * x53 * x366);
	const FLT x370 = 1.0/2.0 * (1. / (x55 * sqrt(x55)));
	const FLT x371 = x60 * x370;
	const FLT x372 = x371 * x369;
	const FLT x373 = x62 * x372;
	const FLT x374 = x66 * x370;
	const FLT x375 = x374 * x369;
	const FLT x376 = x35 * x375;
	const FLT x377 = x47 * x370;
	const FLT x378 = x377 * x369;
	const FLT x379 = x38 * x375;
	const FLT x380 = x56 * x47;
	const FLT x381 = 0.5 * x62;
	const FLT x382 = x381 * (*_x0).Object.Pose.Rot[3];
	const FLT x383 = x67 * x38;
	const FLT x384 = x381 * (*_x0).Object.Pose.Rot[2];
	const FLT x385 = 0.5 * x76;
	const FLT x386 = x381 * (*_x0).Object.Pose.Rot[1];
	const FLT x387 = x67 * x35;
	const FLT x388 = (-1 * x387 * x386) + (x385 * (*_x0).Object.Pose.Rot[0]) + (-1 * x382 * x380) + (-1 * x383 * x384);
	const FLT x389 = (-1 * x70 * x379) + (x72 * x378) + x388 + (-1 * x54 * x373) + (-1 * x63 * x376);
	const FLT x390 = 2 * x88;
	const FLT x391 = x382 * x387;
	const FLT x392 = x381 * (*_x0).Object.Pose.Rot[0];
	const FLT x393 = x392 * x383;
	const FLT x394 = x381 * x380;
	const FLT x395 = x394 * (*_x0).Object.Pose.Rot[1];
	const FLT x396 = x385 * (*_x0).Object.Pose.Rot[2];
	const FLT x397 = (-1 * x396) + (-1 * x395) + x391 + (-1 * x393);
	const FLT x398 = x397 + (x65 * x379) + (-1 * x72 * x376) + (-1 * x70 * x372) + (-1 * x63 * x378);
	const FLT x399 = x392 * x387;
	const FLT x400 = x394 * (*_x0).Object.Pose.Rot[2];
	const FLT x401 = x383 * x382;
	const FLT x402 = x385 * (*_x0).Object.Pose.Rot[1];
	const FLT x403 = (-1 * x401) + (-1 * x402) + (-1 * x399) + x400;
	const FLT x404 = x403 + (x72 * x379) + (x70 * x378) + (x65 * x376) + (-1 * x63 * x372);
	const FLT x405 = x384 * x387;
	const FLT x406 = x383 * x386;
	const FLT x407 = x394 * (*_x0).Object.Pose.Rot[0];
	const FLT x408 = x385 * (*_x0).Object.Pose.Rot[3];
	const FLT x409 = (-1 * x52 * x373) + x408 + x407 + (-1 * x63 * x379) + (-1 * x406) + x405 + (x70 * x376) + (-1 * x65 * x378);
	const FLT x410 = (x409 * sensor_pt[2]) + (-1 * x398 * sensor_pt[1]) + (x404 * sensor_pt[0]);
	const FLT x411 = 2 * x74;
	const FLT x412 = 2 * x93;
	const FLT x413 = (-1 * x409 * sensor_pt[0]) + (x389 * sensor_pt[1]) + (x404 * sensor_pt[2]);
	const FLT x414 = 2 * x83;
	const FLT x415 = (x412 * x398) + (-1 * x413 * x414) + (-1 * x390 * x389) + (x410 * x411);
	const FLT x416 = 2 * x84;
	const FLT x417 = 2 * x87;
	const FLT x418 = (x398 * sensor_pt[0]) + (-1 * x389 * sensor_pt[2]) + (x404 * sensor_pt[1]);
	const FLT x419 = (x409 * x390) + (-1 * x416 * x398) + (-1 * x411 * x418) + (x413 * x417);
	const FLT x420 = (x414 * x418) + (x416 * x389) + (-1 * x409 * x412) + (-1 * x410 * x417);
	const FLT x421 = (-1 * x95 * x420) + (x97 * x415) + (x92 * x419);
	const FLT x422 = (x92 * x415) + (-1 * x97 * x419) + (x420 * x124);
	const FLT x423 = x420 + (x421 * x123) + (-1 * x422 * x164);
	const FLT x424 = (x95 * x419) + (-1 * x415 * x124) + (x92 * x420);
	const FLT x425 = x419 + (x422 * x191) + (-1 * x424 * x123);
	const FLT x426 = x415 + (x424 * x164) + (-1 * x421 * x191);
	const FLT x427 = (-1 * x223 * ((-1 * ((x423 * x221) + (x426 * x220)) * x222) + (x425 * x219))) + (-1 * ((x426 * x201) + (-1 * x423 * x207)) * x216);
	const FLT x428 = 1.0 * x53;
	const FLT x429 = (x368 * (*_x0).Object.Pose.Rot[0]) + (-1 * x428 * (*_x0).Object.Pose.Rot[2]) + (-1 * x365 * (*_x0).Object.Pose.Rot[3]) + (x51 * x366);
	const FLT x430 = x63 * x429;
	const FLT x431 = x35 * x374;
	const FLT x432 = x429 * x377;
	const FLT x433 = x429 * x371;
	const FLT x434 = x38 * x374;
	const FLT x435 = x429 * x434;
	const FLT x436 = x406 + (-1 * x407) + (-1 * x408) + (-1 * x405);
	const FLT x437 = (-1 * x70 * x435) + (-1 * x65 * x433) + x436 + (-1 * x430 * x431) + (x72 * x432);
	const FLT x438 = x429 * x431;
	const FLT x439 = (-1 * x400) + (-1 * x430 * x377) + x402 + (x65 * x435) + (-1 * x72 * x438) + (-1 * x70 * x433) + x401 + x399;
	const FLT x440 = x397 + (-1 * x63 * x433) + (x72 * x435) + (x65 * x438) + (x70 * x432);
	const FLT x441 = (-1 * x434 * x430) + (-1 * x72 * x433) + (x70 * x438) + x388 + (-1 * x65 * x432);
	const FLT x442 = (x441 * sensor_pt[2]) + (-1 * x439 * sensor_pt[1]) + (x440 * sensor_pt[0]);
	const FLT x443 = (x437 * sensor_pt[1]) + (-1 * x441 * sensor_pt[0]) + (x440 * sensor_pt[2]);
	const FLT x444 = (-1 * x414 * x443) + (x411 * x442) + (-1 * x437 * x390) + (x412 * x439);
	const FLT x445 = (x439 * sensor_pt[0]) + (-1 * x437 * sensor_pt[2]) + (x440 * sensor_pt[1]);
	const FLT x446 = (x417 * x443) + (x441 * x390) + (-1 * x416 * x439) + (-1 * x411 * x445);
	const FLT x447 = (x416 * x437) + (-1 * x417 * x442) + (-1 * x412 * x441) + (x414 * x445);
	const FLT x448 = (-1 * x95 * x447) + (x97 * x444) + (x92 * x446);
	const FLT x449 = (-1 * x97 * x446) + (x447 * x124) + (x92 * x444);
	const FLT x450 = x447 + (x448 * x123) + (-1 * x449 * x164);
	const FLT x451 = (-1 * x444 * x124) + (x92 * x447) + (x95 * x446);
	const FLT x452 = (x449 * x191) + x446 + (-1 * x451 * x123);
	const FLT x453 = (x451 * x164) + x444 + (-1 * x448 * x191);
	const FLT x454 = (-1 * x223 * ((-1 * ((x450 * x221) + (x453 * x220)) * x222) + (x452 * x219))) + (-1 * ((x453 * x201) + (-1 * x450 * x207)) * x216);
	const FLT x455 = (-1 * x52 * x366) + (-1 * x428 * (*_x0).Object.Pose.Rot[3]) + (x365 * (*_x0).Object.Pose.Rot[2]) + (x367 * (*_x0).Object.Pose.Rot[0]);
	const FLT x456 = x455 * x374;
	const FLT x457 = x35 * x456;
	const FLT x458 = x455 * x371;
	const FLT x459 = x62 * x458;
	const FLT x460 = x455 * x377;
	const FLT x461 = x38 * x456;
	const FLT x462 = (-1 * x70 * x461) + x393 + (-1 * x391) + (-1 * x54 * x459) + (-1 * x63 * x457) + (x72 * x460) + x395 + x396;
	const FLT x463 = (x72 * x461) + (x70 * x460) + x436 + (-1 * x63 * x458) + (x65 * x457);
	const FLT x464 = (-1 * x52 * x459) + x403 + (-1 * x65 * x460) + (-1 * x63 * x461) + (x70 * x457);
	const FLT x465 = (x462 * sensor_pt[1]) + (x463 * sensor_pt[2]) + (-1 * x464 * sensor_pt[0]);
	const FLT x466 = (-1 * x70 * x458) + (-1 * x63 * x460) + x388 + (x65 * x461) + (-1 * x72 * x457);
	const FLT x467 = (x464 * sensor_pt[2]) + (-1 * x466 * sensor_pt[1]) + (x463 * sensor_pt[0]);
	const FLT x468 = (x467 * x411) + (x466 * x412) + (-1 * x462 * x390) + (-1 * x465 * x414);
	const FLT x469 = (x466 * sensor_pt[0]) + (-1 * x462 * sensor_pt[2]) + (x463 * sensor_pt[1]);
	const FLT x470 = (x465 * x417) + (x464 * x390) + (-1 * x466 * x416) + (-1 * x469 * x411);
	const FLT x471 = (x469 * x414) + (x462 * x416) + (-1 * x464 * x412) + (-1 * x467 * x417);
	const FLT x472 = (-1 * x95 * x471) + (x97 * x468) + (x92 * x470);
	const FLT x473 = (-1 * x97 * x470) + (x471 * x124) + (x92 * x468);
	const FLT x474 = x471 + (x472 * x123) + (-1 * x473 * x164);
	const FLT x475 = (x95 * x470) + (-1 * x468 * x124) + (x92 * x471);
	const FLT x476 = x470 + (x473 * x191) + (-1 * x475 * x123);
	const FLT x477 = x468 + (x475 * x164) + (-1 * x472 * x191);
	const FLT x478 = (-1 * x223 * ((-1 * ((x474 * x221) + (x477 * x220)) * x222) + (x476 * x219))) + (-1 * ((x477 * x201) + (-1 * x474 * x207)) * x216);
	const FLT x479 = x310 + x313;
	const FLT x480 = -1 * x317;
	const FLT x481 = -1 * x316;
	const FLT x482 = 1 + x481 + x480;
	const FLT x483 = x329 + x321;
	const FLT x484 = (-1 * x223 * ((-1 * ((x479 * x221) + (x483 * x220)) * x222) + (x482 * x219))) + (-1 * ((x483 * x201) + (-1 * x479 * x207)) * x216);
	const FLT x485 = x337 + x326;
	const FLT x486 = x321 + x319;
	const FLT x487 = 1 + (-1 * x331);
	const FLT x488 = x487 + x480;
	const FLT x489 = (-1 * x223 * ((-1 * ((x485 * x221) + (x488 * x220)) * x222) + (x486 * x219))) + (-1 * ((x488 * x201) + (-1 * x485 * x207)) * x216);
	const FLT x490 = x487 + x481;
	const FLT x491 = x314 + x310;
	const FLT x492 = x326 + x325;
	const FLT x493 = (-1 * x223 * ((-1 * ((x490 * x221) + (x492 * x220)) * x222) + (x491 * x219))) + (-1 * ((x492 * x201) + (-1 * x490 * x207)) * x216);
	const FLT x494 = dt * dt * dt;
	const FLT x495 = 0.5 * x58 * x494;
	const FLT x496 = x31 * x35;
	const FLT x497 = x495 * x496;
	const FLT x498 = x85 * x497;
	const FLT x499 = x38 * x495;
	const FLT x500 = x35 * x499;
	const FLT x501 = x71 * x500;
	const FLT x502 = x36 * x495;
	const FLT x503 = x35 * x35 * x35;
	const FLT x504 = dt * dt * dt * dt;
	const FLT x505 = 2 * x57 * (1. / (x41 * x41));
	const FLT x506 = x504 * x505;
	const FLT x507 = x33 * x35;
	const FLT x508 = 1.0 * x60;
	const FLT x509 = (1. / (x41 * sqrt(x41))) * x44;
	const FLT x510 = x508 * x509;
	const FLT x511 = x510 * x504;
	const FLT x512 = x39 * x506;
	const FLT x513 = 2 * x59;
	const FLT x514 = x32 * x513;
	const FLT x515 = x39 * x511;
	const FLT x516 = x45 * x508;
	const FLT x517 = x32 * x35;
	const FLT x518 = (-1 * x517 * x516) + (x511 * x503) + (x35 * x515) + (x511 * x507) + (-1 * x503 * x506) + (-1 * x35 * x512) + (x35 * x514) + (-1 * x506 * x507);
	const FLT x519 = 1.0/2.0 * (1. / (x61 * sqrt(x61)));
	const FLT x520 = x519 * x383;
	const FLT x521 = x518 * x520;
	const FLT x522 = x519 * x380;
	const FLT x523 = x518 * x522;
	const FLT x524 = 0.5 * x86;
	const FLT x525 = x45 * x517;
	const FLT x526 = x69 * x519;
	const FLT x527 = x518 * x526;
	const FLT x528 = x494 * x509;
	const FLT x529 = x38 * x528;
	const FLT x530 = x35 * x529;
	const FLT x531 = x75 * x530;
	const FLT x532 = x519 * x387;
	const FLT x533 = x518 * x532;
	const FLT x534 = x496 * x528;
	const FLT x535 = x79 * x534;
	const FLT x536 = x36 * x528;
	const FLT x537 = (-1 * x64 * x536) + (-1 * x51 * x521) + (x77 * x502) + (-1 * x53 * x533) + x501 + x82 + x535 + (x52 * x523) + (-1 * x531) + (-1 * x524 * x525) + (-1 * x498) + (-1 * x54 * x527);
	const FLT x538 = 0.5 * x525;
	const FLT x539 = (-1 * x64 * x534) + (x77 * x497);
	const FLT x540 = (-1 * x81 * x500) + (x86 * x530);
	const FLT x541 = (-1 * x51 * x527) + x539 + (-1 * x75 * x538) + (x85 * x502) + x540 + (x54 * x521) + (-1 * x79 * x536) + x73 + (-1 * x53 * x523) + (-1 * x52 * x533);
	const FLT x542 = -1 * x68;
	const FLT x543 = x79 * x530;
	const FLT x544 = x85 * x500;
	const FLT x545 = (x75 * x534) + (-1 * x71 * x497);
	const FLT x546 = x545 + (-1 * x81 * x502) + x543 + (-1 * x64 * x538) + (x54 * x533) + (-1 * x544) + (x86 * x536) + x542 + (-1 * x53 * x527) + (x52 * x521) + (x51 * x523);
	const FLT x547 = -1 * x80;
	const FLT x548 = x86 * x534;
	const FLT x549 = x81 * x497;
	const FLT x550 = (-1 * x64 * x530) + (x77 * x500);
	const FLT x551 = x550 + x549 + (x75 * x536) + (-1 * x548) + (-1 * x52 * x527) + (x51 * x533) + (-1 * x79 * x538) + (-1 * x54 * x523) + (-1 * x71 * x502) + x547 + (-1 * x53 * x521);
	const FLT x552 = 2 * ((-1 * x541 * sensor_pt[1]) + (x551 * sensor_pt[2]) + (x546 * sensor_pt[0]));
	const FLT x553 = (x537 * sensor_pt[1]) + (-1 * x551 * sensor_pt[0]) + (x546 * sensor_pt[2]);
	const FLT x554 = (-1 * x414 * x553) + (x74 * x552) + (-1 * x537 * x390) + (x412 * x541);
	const FLT x555 = (x541 * sensor_pt[0]) + (-1 * x537 * sensor_pt[2]) + (x546 * sensor_pt[1]);
	const FLT x556 = (x551 * x390) + (-1 * x411 * x555) + (-1 * x416 * x541) + (x417 * x553);
	const FLT x557 = (x414 * x555) + (x416 * x537) + (-1 * x412 * x551) + (-1 * x87 * x552);
	const FLT x558 = (-1 * x95 * x557) + (x97 * x554) + (x92 * x556);
	const FLT x559 = (-1 * x97 * x556) + (x557 * x124) + (x92 * x554);
	const FLT x560 = x557 + (x558 * x123) + (-1 * x559 * x164);
	const FLT x561 = (x95 * x556) + (-1 * x554 * x124) + (x92 * x557);
	const FLT x562 = x556 + (x559 * x191) + (-1 * x561 * x123);
	const FLT x563 = x554 + (x561 * x164) + (-1 * x558 * x191);
	const FLT x564 = (-1 * x223 * ((-1 * ((x560 * x221) + (x563 * x220)) * x222) + (x562 * x219))) + (-1 * ((x563 * x201) + (-1 * x560 * x207)) * x216);
	const FLT x565 = x39 * x495;
	const FLT x566 = x39 * x528;
	const FLT x567 = x38 * x506;
	const FLT x568 = x38 * x511;
	const FLT x569 = x32 * x38;
	const FLT x570 = (x38 * x38 * x38) * x504;
	const FLT x571 = (x513 * x569) + (x510 * x570) + (x36 * x568) + (-1 * x36 * x567) + (-1 * x516 * x569) + (x33 * x568) + (-1 * x505 * x570) + (-1 * x33 * x567);
	const FLT x572 = x532 * x571;
	const FLT x573 = x53 * x571;
	const FLT x574 = x54 * x571;
	const FLT x575 = x526 * x571;
	const FLT x576 = x45 * x569;
	const FLT x577 = 0.5 * x576;
	const FLT x578 = x31 * x529;
	const FLT x579 = x31 * x499;
	const FLT x580 = (x77 * x579) + (-1 * x64 * x578);
	const FLT x581 = x542 + x580 + (-1 * x75 * x577) + (-1 * x51 * x575) + (-1 * x543) + (x520 * x574) + (x86 * x566) + (-1 * x81 * x565) + x544 + (-1 * x52 * x572) + (-1 * x522 * x573);
	const FLT x582 = x86 * x578;
	const FLT x583 = x81 * x579;
	const FLT x584 = (x51 * x572) + (x77 * x565) + (-1 * x582) + (-1 * x64 * x566) + x82 + (-1 * x501) + x531 + (-1 * x52 * x575) + (-1 * x522 * x574) + (-1 * x520 * x573) + (-1 * x79 * x577) + x583;
	const FLT x585 = -1 * x73;
	const FLT x586 = x75 * x578;
	const FLT x587 = x71 * x579;
	const FLT x588 = x51 * x571;
	const FLT x589 = x52 * x571;
	const FLT x590 = (x532 * x574) + (x522 * x588) + (-1 * x53 * x575) + (-1 * x85 * x565) + x586 + x540 + (x520 * x589) + x585 + (-1 * x64 * x577) + (-1 * x587) + (x79 * x566);
	const FLT x591 = (x79 * x578) + (-1 * x85 * x579);
	const FLT x592 = x550 + (-1 * x524 * x576) + (-1 * x520 * x588) + x591 + x80 + (-1 * x54 * x575) + (-1 * x532 * x573) + (-1 * x75 * x566) + (x71 * x565) + (x522 * x589);
	const FLT x593 = (x592 * sensor_pt[1]) + (-1 * x584 * sensor_pt[0]) + (x590 * sensor_pt[2]);
	const FLT x594 = (x581 * sensor_pt[0]) + (-1 * x592 * sensor_pt[2]) + (x590 * sensor_pt[1]);
	const FLT x595 = (-1 * x411 * x594) + (-1 * x416 * x581) + (x584 * x390) + (x417 * x593);
	const FLT x596 = (x584 * sensor_pt[2]) + (-1 * x581 * sensor_pt[1]) + (x590 * sensor_pt[0]);
	const FLT x597 = (x411 * x596) + (-1 * x414 * x593) + (-1 * x592 * x390) + (x412 * x581);
	const FLT x598 = (-1 * x417 * x596) + (x414 * x594) + (-1 * x412 * x584) + (x416 * x592);
	const FLT x599 = (-1 * x95 * x598) + (x92 * x595) + (x97 * x597);
	const FLT x600 = (-1 * x97 * x595) + (x598 * x124) + (x92 * x597);
	const FLT x601 = x598 + (x599 * x123) + (-1 * x600 * x164);
	const FLT x602 = (-1 * x597 * x124) + (x95 * x595) + (x92 * x598);
	const FLT x603 = x595 + (x600 * x191) + (-1 * x602 * x123);
	const FLT x604 = x597 + (-1 * x599 * x191) + (x602 * x164);
	const FLT x605 = (-1 * x223 * ((-1 * ((x601 * x221) + (x604 * x220)) * x222) + (x603 * x219))) + (-1 * ((x604 * x201) + (-1 * x601 * x207)) * x216);
	const FLT x606 = x33 * x528;
	const FLT x607 = x46 * x32;
	const FLT x608 = x33 * x495;
	const FLT x609 = x31 * x36;
	const FLT x610 = x31 * x31 * x31;
	const FLT x611 = (x610 * x511) + (-1 * x610 * x506) + (x31 * x515) + (x609 * x511) + (-1 * x609 * x506) + (-1 * x607 * x508) + (x31 * x514) + (-1 * x31 * x512);
	const FLT x612 = x611 * x519;
	const FLT x613 = x612 * x387;
	const FLT x614 = x611 * x526;
	const FLT x615 = x612 * x380;
	const FLT x616 = x612 * x383;
	const FLT x617 = x539 + x587 + (-1 * x607 * x524) + (-1 * x51 * x616) + (x52 * x615) + x585 + (x79 * x606) + (-1 * x54 * x614) + (-1 * x53 * x613) + (-1 * x586) + (-1 * x85 * x608);
	const FLT x618 = 0.5 * x607;
	const FLT x619 = (-1 * x64 * x606) + (-1 * x53 * x615) + x498 + (-1 * x583) + (-1 * x52 * x613) + (-1 * x535) + x582 + (x54 * x616) + x82 + (x77 * x608) + (-1 * x75 * x618) + (-1 * x51 * x614);
	const FLT x620 = x591 + (-1 * x64 * x618) + (x75 * x606) + (x54 * x613) + (-1 * x71 * x608) + (x52 * x616) + (x51 * x615) + x548 + (-1 * x549) + (-1 * x53 * x614) + x547;
	const FLT x621 = x545 + (-1 * x86 * x606) + x580 + x68 + (x51 * x613) + (-1 * x52 * x614) + (-1 * x79 * x618) + (-1 * x54 * x615) + (-1 * x53 * x616) + (x81 * x608);
	const FLT x622 = (x617 * sensor_pt[1]) + (x620 * sensor_pt[2]) + (-1 * x621 * sensor_pt[0]);
	const FLT x623 = (x621 * sensor_pt[2]) + (-1 * x619 * sensor_pt[1]) + (x620 * sensor_pt[0]);
	const FLT x624 = (x411 * x623) + (-1 * x414 * x622) + (-1 * x617 * x390) + (x412 * x619);
	const FLT x625 = (x619 * sensor_pt[0]) + (-1 * x617 * sensor_pt[2]) + (x620 * sensor_pt[1]);
	const FLT x626 = (x417 * x622) + (x621 * x390) + (-1 * x416 * x619) + (-1 * x411 * x625);
	const FLT x627 = (x414 * x625) + (-1 * x417 * x623) + (-1 * x412 * x621) + (x416 * x617);
	const FLT x628 = x91 * x627;
	const FLT x629 = (x97 * x624) + (-1 * x6 * x628) + (x92 * x626);
	const FLT x630 = (x3 * x628) + (-1 * x97 * x626) + (x92 * x624);
	const FLT x631 = x627 + (x629 * x123) + (-1 * x630 * x164);
	const FLT x632 = (-1 * x624 * x124) + (x95 * x626) + (x92 * x627);
	const FLT x633 = x626 + (x630 * x191) + (-1 * x632 * x123);
	const FLT x634 = x624 + (x632 * x164) + (-1 * x629 * x191);
	const FLT x635 = (-1 * x223 * ((-1 * ((x631 * x221) + (x634 * x220)) * x222) + (x633 * x219))) + (-1 * ((x634 * x201) + (-1 * x631 * x207)) * x216);
	const FLT x636 = dt * x313;
	const FLT x637 = dt * x310;
	const FLT x638 = x637 + x636;
	const FLT x639 = -1 * dt * x316;
	const FLT x640 = (-1 * dt * x317) + dt;
	const FLT x641 = x640 + x639;
	const FLT x642 = dt * x321;
	const FLT x643 = dt * x319;
	const FLT x644 = (-1 * x643) + x642;
	const FLT x645 = (-1 * x223 * ((-1 * ((x638 * x221) + (x644 * x220)) * x222) + (x641 * x219))) + (-1 * ((x644 * x201) + (-1 * x638 * x207)) * x216);
	const FLT x646 = dt * x326;
	const FLT x647 = dt * x325;
	const FLT x648 = (-1 * x647) + x646;
	const FLT x649 = x642 + x643;
	const FLT x650 = -1 * dt * x331;
	const FLT x651 = x640 + x650;
	const FLT x652 = (-1 * x223 * ((-1 * ((x648 * x221) + (x651 * x220)) * x222) + (x649 * x219))) + (-1 * ((x651 * x201) + (-1 * x648 * x207)) * x216);
	const FLT x653 = x650 + x639 + dt;
	const FLT x654 = (-1 * x636) + x637;
	const FLT x655 = x646 + x647;
	const FLT x656 = (-1 * x223 * ((-1 * ((x653 * x221) + (x655 * x220)) * x222) + (x654 * x219))) + (-1 * ((x655 * x201) + (-1 * x653 * x207)) * x216);
	const FLT x657 = x217 * x223 * x177;
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), x224 + (((-1 * x200 * x201) + (x174 * x181)) * x205) + (x224 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), x266 + (((-1 * x201 * x263) + (x253 * x181)) * x205) + (x266 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), (((-1 * x201 * x303) + (x298 * x181)) * x205) + x305 + (x226 * x305));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), x324 + (((-1 * x201 * x318) + (x315 * x181)) * x205) + (x226 * x324));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), x334 + (((-1 * x201 * x330) + (x328 * x181)) * x205) + (x226 * x334));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), x339 + (((-1 * x201 * x336) + (x335 * x181)) * x205) + (x226 * x339));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), x353 + (((-1 * x201 * x348) + (x345 * x181)) * x205) + (x226 * x353));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), x360 + (((-1 * x201 * x357) + (x356 * x181)) * x205) + (x226 * x360));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), x364 + (((-1 * x201 * x362) + (x361 * x181)) * x205) + (x226 * x364));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), (((-1 * x425 * x201) + (x423 * x181)) * x205) + x427 + (x427 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), x454 + (((-1 * x452 * x201) + (x450 * x181)) * x205) + (x454 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), x478 + (((-1 * x476 * x201) + (x474 * x181)) * x205) + (x478 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), x484 + (((-1 * x482 * x201) + (x479 * x181)) * x205) + (x484 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), (((-1 * x486 * x201) + (x485 * x181)) * x205) + x489 + (x489 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), x493 + (((-1 * x491 * x201) + (x490 * x181)) * x205) + (x493 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x564 + (((-1 * x562 * x201) + (x560 * x181)) * x205) + (x564 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x605 + (((-1 * x603 * x201) + (x601 * x181)) * x205) + (x605 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), (((-1 * x633 * x201) + (x631 * x181)) * x205) + x635 + (x635 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), x645 + (((-1 * x641 * x201) + (x638 * x181)) * x205) + (x645 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), x652 + (((-1 * x649 * x201) + (x648 * x181)) * x205) + (x652 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), x656 + (((-1 * x654 * x201) + (x653 * x181)) * x205) + (x656 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.curve)/sizeof(FLT), x203 * x203);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), -1 * cos(x225));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), x226);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.phase)/sizeof(FLT), -1 + (-1 * x226));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.tilt)/sizeof(FLT), (-1 * x657 * x226) + (-1 * x657));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]

static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_error_model(Hx, dt, _x0, error_model, sensor_pt);
    }
}
// Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]]
static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x2 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x3 = (x1 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x0 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2];
	const FLT x4 = dt * dt;
	const FLT x5 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x6 = x4 * (x5 * x5);
	const FLT x7 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x8 = x4 * (x7 * x7);
	const FLT x9 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x10 = x4 * (x9 * x9);
	const FLT x11 = 1e-10 + x10 + x6 + x8;
	const FLT x12 = sqrt(x11);
	const FLT x13 = 0.5 * x12;
	const FLT x14 = cos(x13);
	const FLT x15 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x16 = (x2 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x15 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x1 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x17 = (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x15 * (*error_model).Object.Pose.AxisAngleRot[1]);
	const FLT x18 = (*_x0).Object.Pose.Rot[1] + (x1 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x15 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x19 = sin(x13);
	const FLT x20 = (1. / x11) * (x19 * x19);
	const FLT x21 = (1. / sqrt((x18 * x18) + (x17 * x17) + (x16 * x16) + (x3 * x3))) * (1. / sqrt((x8 * x20) + (x14 * x14) + (x6 * x20) + (x20 * x10)));
	const FLT x22 = x21 * x14;
	const FLT x23 = x3 * x22;
	const FLT x24 = dt * x21 * (1. / x12) * x19;
	const FLT x25 = x9 * x24;
	const FLT x26 = x25 * x17;
	const FLT x27 = x24 * x16;
	const FLT x28 = x7 * x27;
	const FLT x29 = x5 * x24;
	const FLT x30 = x29 * x18;
	const FLT x31 = (-1 * x30) + x28 + (-1 * x23) + (-1 * x26);
	const FLT x32 = x3 * x29;
	const FLT x33 = x9 * x27;
	const FLT x34 = x22 * x18;
	const FLT x35 = x7 * x24;
	const FLT x36 = x35 * x17;
	const FLT x37 = x34 + x36 + (-1 * x32) + x33;
	const FLT x38 = 2 * x37;
	const FLT x39 = (-1 * x35 * x18) + (x22 * x17) + (-1 * x5 * x27) + (-1 * x3 * x25);
	const FLT x40 = x29 * x17;
	const FLT x41 = x25 * x18;
	const FLT x42 = x22 * x16;
	const FLT x43 = x3 * x35;
	const FLT x44 = x43 + x42 + x40 + (-1 * x41);
	const FLT x45 = 2 * x44;
	const FLT x46 = x45 * x39;
	const FLT x47 = x46 + (-1 * x31 * x38);
	const FLT x48 = 0.5 * (*_x0).Lighthouse.Rot[2];
	const FLT x49 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x50 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x51 = (x50 * (*_x0).Lighthouse.Rot[1]) + (x49 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x48 * (*error_model).Lighthouse.AxisAngleRot[0]);
	const FLT x52 = x51 * x51;
	const FLT x53 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x54 = (-1 * x53 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x49 * (*_x0).Lighthouse.Rot[3]) + (-1 * x48 * (*error_model).Lighthouse.AxisAngleRot[1]);
	const FLT x55 = x54 * x54;
	const FLT x56 = (x50 * (*_x0).Lighthouse.Rot[0]) + (x53 * (*_x0).Lighthouse.Rot[3]) + (-1 * x49 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[2];
	const FLT x57 = x56 * x56;
	const FLT x58 = (*_x0).Lighthouse.Rot[1] + (x53 * (*_x0).Lighthouse.Rot[0]) + (-1 * x50 * (*_x0).Lighthouse.Rot[3]) + (x49 * (*_x0).Lighthouse.Rot[2]);
	const FLT x59 = x58 * x58;
	const FLT x60 = x57 + x59 + x52 + x55;
	const FLT x61 = 1. / x60;
	const FLT x62 = (1. / sqrt((x61 * x59) + (x61 * x52) + (x61 * x55) + (x61 * x57))) * (1. / sqrt(x60));
	const FLT x63 = x62 * x51;
	const FLT x64 = (-1 * x28) + x30 + x26 + x23;
	const FLT x65 = 2 * x64;
	const FLT x66 = 1 + (x65 * x31) + (-2 * (x44 * x44));
	const FLT x67 = x62 * x54;
	const FLT x68 = x65 * x39;
	const FLT x69 = (x44 * x38) + (-1 * x68);
	const FLT x70 = x62 * x56;
	const FLT x71 = (-1 * x70 * x69) + (x63 * x47) + (x67 * x66);
	const FLT x72 = 2 * x70;
	const FLT x73 = x62 * x58;
	const FLT x74 = (-1 * x63 * x66) + (x73 * x69) + (x67 * x47);
	const FLT x75 = 2 * x73;
	const FLT x76 = x69 + (x71 * x72) + (-1 * x75 * x74);
	const FLT x77 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x78 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x79 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x80 = (x79 * x63) + (-1 * x70 * x77) + (x78 * x67);
	const FLT x81 = (x79 * x67) + (x73 * x77) + (-1 * x78 * x63);
	const FLT x82 = 1.0/2.0 * dt * fabs(dt);
	const FLT x83 = (-1 * x37 * sensor_pt[2]) + (x39 * sensor_pt[1]) + (x44 * sensor_pt[0]);
	const FLT x84 = (-1 * x64 * sensor_pt[0]) + (x39 * sensor_pt[2]) + (x37 * sensor_pt[1]);
	const FLT x85 = (*_x0).Object.Pose.Pos[0] + (x82 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + (2 * ((x84 * x64) + (-1 * x83 * x44)));
	const FLT x86 = x85 * x62;
	const FLT x87 = (-1 * x44 * sensor_pt[1]) + (x39 * sensor_pt[0]) + (x64 * sensor_pt[2]);
	const FLT x88 = (*_x0).Object.Pose.Pos[2] + (2 * ((x83 * x37) + (-1 * x87 * x64))) + (x82 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2]));
	const FLT x89 = x88 * x62;
	const FLT x90 = (*_x0).Object.Pose.Pos[1] + (x82 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x87 * x44) + (-1 * x84 * x37)));
	const FLT x91 = x62 * ((x63 * x90) + (x86 * x54) + (-1 * x89 * x56));
	const FLT x92 = (x89 * x58) + (x67 * x90) + (-1 * x86 * x51);
	const FLT x93 = x88 + (-1 * (x77 + (2 * ((-1 * x81 * x73) + (x80 * x70))))) + (2 * ((-1 * x73 * x92) + (x56 * x91)));
	const FLT x94 = x93 * x93;
	const FLT x95 = 1. / x94;
	const FLT x96 = (x77 * x67) + (x70 * x78) + (-1 * x73 * x79);
	const FLT x97 = (x86 * x56) + (x89 * x54) + (-1 * x73 * x90);
	const FLT x98 = x85 + (-1 * (x78 + (2 * ((-1 * x70 * x96) + (x81 * x63))))) + (2 * ((-1 * x70 * x97) + (x63 * x92)));
	const FLT x99 = x98 * x95;
	const FLT x100 = 1. / x93;
	const FLT x101 = (-1 * x73 * x47) + (x70 * x66) + (x67 * x69);
	const FLT x102 = 2 * x63;
	const FLT x103 = x66 + (-1 * x72 * x101) + (x74 * x102);
	const FLT x104 = -1 * x93;
	const FLT x105 = x98 * x98;
	const FLT x106 = 2 * x94 * atan2(x98, x104) * (1. / (x94 + x105)) * ((*error_model).BSD1.curve + (*_x0).BSD1.curve);
	const FLT x107 = x90 + (2 * ((-1 * x51 * x91) + (x73 * x97))) + (-1 * (x79 + (2 * ((-1 * x80 * x63) + (x73 * x96)))));
	const FLT x108 = x95 * x107;
	const FLT x109 = x47 + (-1 * x71 * x102) + (x75 * x101);
	const FLT x110 = (x107 * x107) + x94;
	const FLT x111 = 1. / x110;
	const FLT x112 = x94 * x111;
	const FLT x113 = (*error_model).BSD1.tilt + (*_x0).BSD1.tilt;
	const FLT x114 = (1. / sqrt(x110)) * x113;
	const FLT x115 = 2 * x107;
	const FLT x116 = 2 * x93;
	const FLT x117 = 1.0/2.0 * x98 * (1. / (x110 * sqrt(x110))) * x113;
	const FLT x118 = 1. / sqrt(1 + (-1 * x105 * x111 * (x113 * x113)));
	const FLT x119 = (-1 * x118 * ((-1 * x117 * ((x76 * x116) + (x109 * x115))) + (x103 * x114))) + (-1 * x112 * ((x100 * x109) + (-1 * x76 * x108)));
	const FLT x120 = sin(1.5707963267949 + (*error_model).BSD1.gibpha + (-1 * atan2(-1 * x107, x104)) + (-1 * ((*error_model).BSD1.phase + (*_x0).BSD1.phase)) + (*_x0).BSD1.gibpha + (-1 * asin(x98 * x114))) * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag);
	const FLT x121 = (-1 * x40) + (-1 * x43) + x41 + (-1 * x42);
	const FLT x122 = 1 + (x45 * x121) + (-2 * (x37 * x37));
	const FLT x123 = (x65 * x37) + (-1 * x46);
	const FLT x124 = x38 * x39;
	const FLT x125 = x124 + (-1 * x65 * x121);
	const FLT x126 = (-1 * x70 * x125) + (x63 * x122) + (x67 * x123);
	const FLT x127 = (-1 * x63 * x123) + (x73 * x125) + (x67 * x122);
	const FLT x128 = x125 + (x72 * x126) + (-1 * x75 * x127);
	const FLT x129 = (-1 * x73 * x122) + (x70 * x123) + (x67 * x125);
	const FLT x130 = x123 + (-1 * x72 * x129) + (x102 * x127);
	const FLT x131 = (-1 * x102 * x126) + x122 + (x75 * x129);
	const FLT x132 = (-1 * x118 * ((-1 * ((x116 * x128) + (x115 * x131)) * x117) + (x114 * x130))) + (-1 * ((x100 * x131) + (-1 * x108 * x128)) * x112);
	const FLT x133 = (x65 * x44) + (-1 * x124);
	const FLT x134 = (-1 * x34) + (-1 * x33) + (-1 * x36) + x32;
	const FLT x135 = x68 + (-1 * x45 * x134);
	const FLT x136 = 1 + (x38 * x134) + (-2 * (x64 * x64));
	const FLT x137 = (-1 * x70 * x136) + (x63 * x133) + (x67 * x135);
	const FLT x138 = x62 * x136;
	const FLT x139 = (-1 * x63 * x135) + (x58 * x138) + (x67 * x133);
	const FLT x140 = x136 + (x72 * x137) + (-1 * x75 * x139);
	const FLT x141 = x95 * x140;
	const FLT x142 = (-1 * x73 * x133) + (x70 * x135) + (x54 * x138);
	const FLT x143 = x135 + (-1 * x72 * x142) + (x102 * x139);
	const FLT x144 = x133 + (-1 * x102 * x137) + (x75 * x142);
	const FLT x145 = (-1 * x118 * ((-1 * ((x116 * x140) + (x115 * x144)) * x117) + (x114 * x143))) + (-1 * ((x100 * x144) + (-1 * x107 * x141)) * x112);
	cnMatrixOptionalSet(Hx, 0, 0, x119 + (x106 * ((-1 * x100 * x103) + (x76 * x99))) + (x119 * x120));
	cnMatrixOptionalSet(Hx, 0, 1, x132 + (x106 * ((-1 * x100 * x130) + (x99 * x128))) + (x120 * x132));
	cnMatrixOptionalSet(Hx, 0, 2, x145 + (x106 * ((-1 * x100 * x143) + (x98 * x141))) + (x120 * x145));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen1 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]]

static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen1(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_y_gen1_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt);
    }
}
static inline FLT SurviveJointKalmanErrorModel_LightMeas_x_gen2(const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x1 = 0.5 * (*_x0).Lighthouse.Rot[3];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x3 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x4 = (-1 * x3 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*error_model).Lighthouse.AxisAngleRot[0]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2];
	const FLT x5 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x6 = (x2 * (*_x0).Lighthouse.Rot[1]) + (x3 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x5 * (*_x0).Lighthouse.Rot[2]);
	const FLT x7 = x6 * x6;
	const FLT x8 = (-1 * x5 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x1 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x9 = x8 * x8;
	const FLT x10 = x4 * x4;
	const FLT x11 = (*_x0).Lighthouse.Rot[1] + (x5 * (*_x0).Lighthouse.Rot[0]) + (-1 * x1 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x3 * (*_x0).Lighthouse.Rot[2]);
	const FLT x12 = x11 * x11;
	const FLT x13 = x12 + x7 + x10 + x9;
	const FLT x14 = 1. / x13;
	const FLT x15 = (1. / sqrt((x14 * x12) + (x9 * x14) + (x7 * x14) + (x14 * x10))) * (1. / sqrt(x13));
	const FLT x16 = x4 * x15;
	const FLT x17 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x18 = x8 * x15;
	const FLT x19 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x20 = x6 * x15;
	const FLT x21 = (x20 * x19) + (-1 * x0 * x16) + (x18 * x17);
	const FLT x22 = x15 * x11;
	const FLT x23 = (x0 * x22) + (x19 * x18) + (-1 * x20 * x17);
	const FLT x24 = 1.0/2.0 * dt * fabs(dt);
	const FLT x25 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x26 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x27 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x28 = (-1 * x27 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x25 * (*_x0).Object.Pose.Rot[3]) + (-1 * x26 * (*_x0).Object.Pose.Rot[2]);
	const FLT x29 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x30 = dt * dt;
	const FLT x31 = x30 * (x29 * x29);
	const FLT x32 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x33 = x30 * (x32 * x32);
	const FLT x34 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x35 = x30 * (x34 * x34);
	const FLT x36 = 1e-10 + x35 + x31 + x33;
	const FLT x37 = sqrt(x36);
	const FLT x38 = 0.5 * x37;
	const FLT x39 = sin(x38);
	const FLT x40 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x41 = (-1 * x40 * (*_x0).Object.Pose.Rot[2]) + (x26 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[3] + (x25 * (*_x0).Object.Pose.Rot[0]);
	const FLT x42 = (-1 * x27 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x26 * (*_x0).Object.Pose.Rot[0]) + (x40 * (*_x0).Object.Pose.Rot[3]) + (*_x0).Object.Pose.Rot[2];
	const FLT x43 = (x40 * (*_x0).Object.Pose.Rot[0]) + (*_x0).Object.Pose.Rot[1] + (-1 * x26 * (*_x0).Object.Pose.Rot[3]) + (x25 * (*_x0).Object.Pose.Rot[2]);
	const FLT x44 = (1. / x36) * (x39 * x39);
	const FLT x45 = cos(x38);
	const FLT x46 = (1. / sqrt((x43 * x43) + (x28 * x28) + (x41 * x41) + (x42 * x42))) * (1. / sqrt((x44 * x33) + (x45 * x45) + (x44 * x31) + (x44 * x35)));
	const FLT x47 = dt * x46 * (1. / x37) * x39;
	const FLT x48 = x47 * x29;
	const FLT x49 = x47 * x34;
	const FLT x50 = x45 * x46;
	const FLT x51 = x47 * x32;
	const FLT x52 = (x51 * x42) + (x48 * x28) + (x50 * x41) + (-1 * x43 * x49);
	const FLT x53 = (-1 * x51 * x43) + (-1 * x41 * x48) + (x50 * x28) + (-1 * x42 * x49);
	const FLT x54 = (x50 * x43) + (-1 * x42 * x48) + (x51 * x28) + (x41 * x49);
	const FLT x55 = (-1 * x54 * sensor_pt[2]) + (x53 * sensor_pt[1]) + (x52 * sensor_pt[0]);
	const FLT x56 = (-1 * x51 * x41) + (x43 * x48) + (x49 * x28) + (x50 * x42);
	const FLT x57 = (-1 * x56 * sensor_pt[0]) + (x53 * sensor_pt[2]) + (x54 * sensor_pt[1]);
	const FLT x58 = (*_x0).Object.Pose.Pos[0] + (x24 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0] + (2 * ((x57 * x56) + (-1 * x52 * x55)));
	const FLT x59 = (-1 * x52 * sensor_pt[1]) + (x53 * sensor_pt[0]) + (x56 * sensor_pt[2]);
	const FLT x60 = (*_x0).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (2 * ((x54 * x55) + (-1 * x56 * x59))) + (x24 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2])) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2];
	const FLT x61 = (*_x0).Object.Pose.Pos[1] + (x24 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + (2 * ((x52 * x59) + (-1 * x54 * x57))) + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1]));
	const FLT x62 = (x61 * x20) + (x58 * x18) + (-1 * x60 * x16);
	const FLT x63 = x15 * ((x61 * x18) + (x60 * x22) + (-1 * x58 * x20));
	const FLT x64 = (-1 * (x0 + (2 * ((-1 * x22 * x23) + (x21 * x16))))) + x60 + (2 * ((-1 * x63 * x11) + (x62 * x16)));
	const FLT x65 = (x17 * x16) + (x0 * x18) + (-1 * x22 * x19);
	const FLT x66 = (x60 * x18) + (x58 * x16) + (-1 * x61 * x22);
	const FLT x67 = x58 + (-1 * (x17 + (2 * ((-1 * x65 * x16) + (x23 * x20))))) + (2 * ((-1 * x66 * x16) + (x6 * x63)));
	const FLT x68 = atan2(-1 * x64, x67);
	const FLT x69 = x61 + (2 * ((-1 * x62 * x20) + (x66 * x22))) + (-1 * (x19 + (2 * ((-1 * x20 * x21) + (x65 * x22)))));
	const FLT x70 = (x67 * x67) + (x64 * x64);
	const FLT x71 = 0.523598775598299 + (*error_model).BSD0.tilt + (*_x0).BSD0.tilt;
	const FLT x72 = cos(x71);
	const FLT x73 = asin((1. / x72) * x69 * (1. / sqrt(x70 + (x69 * x69))));
	const FLT x74 = 0.0028679863 + (x73 * (-8.0108022e-06 + (-8.0108022e-06 * x73)));
	const FLT x75 = 5.3685255e-06 + (x73 * x74);
	const FLT x76 = 0.0076069798 + (x73 * x75);
	const FLT x77 = (1. / sqrt(x70)) * x69 * tan(x71);
	const FLT x78 = (*_x0).BSD0.curve + (sin(x68 + (-1 * asin(x77)) + (*error_model).BSD0.ogeephase + (*_x0).BSD0.ogeephase) * ((*error_model).BSD0.ogeemag + (*_x0).BSD0.ogeemag)) + (*error_model).BSD0.curve;
	const FLT x79 = (-1 * asin(x77 + ((x73 * x73) * x78 * x76 * (1. / (x72 + (-1 * x78 * sin(x71) * ((x73 * (x76 + (x73 * (x75 + (x73 * (x74 + (x73 * (-8.0108022e-06 + (-1.60216044e-05 * x73))))))))) + (x73 * x76)))))))) + x68;
	return -1.5707963267949 + x79 + (-1 * ((*error_model).BSD0.phase + (*_x0).BSD0.phase)) + (sin(x79 + (*error_model).BSD0.gibpha + (*_x0).BSD0.gibpha) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag));
}

// Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_x0(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = (*error_model).BSD0.ogeemag + (*_x0).BSD0.ogeemag;
	const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x3 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x4 = (-1 * x3 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[3]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2];
	const FLT x5 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x6 = (x2 * (*_x0).Lighthouse.Rot[1]) + (x3 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x1 * (*_x0).Lighthouse.Rot[2]);
	const FLT x7 = x6 * x6;
	const FLT x8 = (-1 * x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x3 * (*_x0).Lighthouse.Rot[3]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x9 = x8 * x8;
	const FLT x10 = x4 * x4;
	const FLT x11 = (*_x0).Lighthouse.Rot[1] + (x1 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (x3 * (*_x0).Lighthouse.Rot[2]);
	const FLT x12 = x11 * x11;
	const FLT x13 = x12 + x7 + x10 + x9;
	const FLT x14 = 1. / x13;
	const FLT x15 = x7 * x14;
	const FLT x16 = x14 * x10;
	const FLT x17 = x14 * x12;
	const FLT x18 = x17 + (x9 * x14) + x15 + x16;
	const FLT x19 = 1. / sqrt(x18);
	const FLT x20 = 1. / sqrt(x13);
	const FLT x21 = x20 * x19;
	const FLT x22 = x5 * x21;
	const FLT x23 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x24 = x23 * x21;
	const FLT x25 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x26 = x25 * x21;
	const FLT x27 = (x6 * x26) + (-1 * x4 * x22) + (x8 * x24);
	const FLT x28 = x4 * x21;
	const FLT x29 = (x22 * x11) + (x8 * x26) + (-1 * x6 * x24);
	const FLT x30 = x21 * x11;
	const FLT x31 = dt * fabs(dt);
	const FLT x32 = 1.0/2.0 * x31;
	const FLT x33 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x34 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x35 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x36 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x37 = (-1 * x36 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x34 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x35 * (*error_model).Object.Pose.AxisAngleRot[1]);
	const FLT x38 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x39 = (x36 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[3] + (-1 * x35 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x38 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x40 = (x38 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x34 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x36 * (*error_model).Object.Pose.AxisAngleRot[2]) + (*_x0).Object.Pose.Rot[2];
	const FLT x41 = (*_x0).Object.Pose.Rot[1] + (-1 * x34 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x38 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x35 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x42 = (x41 * x41) + (x39 * x39) + (x37 * x37) + (x40 * x40);
	const FLT x43 = 1. / sqrt(x42);
	const FLT x44 = dt * dt;
	const FLT x45 = x33 * x33;
	const FLT x46 = x44 * x45;
	const FLT x47 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x48 = x47 * x47;
	const FLT x49 = x44 * x48;
	const FLT x50 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x51 = x50 * x50;
	const FLT x52 = x51 * x44;
	const FLT x53 = 1e-10 + x52 + x46 + x49;
	const FLT x54 = sqrt(x53);
	const FLT x55 = 0.5 * x54;
	const FLT x56 = sin(x55);
	const FLT x57 = x56 * x56;
	const FLT x58 = 1. / x53;
	const FLT x59 = x58 * x57;
	const FLT x60 = cos(x55);
	const FLT x61 = (x59 * x49) + (x60 * x60) + (x59 * x46) + (x52 * x59);
	const FLT x62 = 1. / sqrt(x61);
	const FLT x63 = x62 * x43;
	const FLT x64 = (1. / x54) * x56;
	const FLT x65 = dt * x64;
	const FLT x66 = x63 * x65;
	const FLT x67 = x66 * x37;
	const FLT x68 = x64 * x50;
	const FLT x69 = dt * x68;
	const FLT x70 = x63 * x69;
	const FLT x71 = x60 * x62;
	const FLT x72 = x71 * x43;
	const FLT x73 = x72 * x39;
	const FLT x74 = x66 * x40;
	const FLT x75 = (x74 * x47) + x73 + (x67 * x33) + (-1 * x70 * x41);
	const FLT x76 = x63 * x39;
	const FLT x77 = x76 * x65;
	const FLT x78 = x72 * x37;
	const FLT x79 = x66 * x41;
	const FLT x80 = (-1 * x79 * x47) + (-1 * x77 * x33) + x78 + (-1 * x70 * x40);
	const FLT x81 = x72 * x41;
	const FLT x82 = (x67 * x47) + x81 + (-1 * x74 * x33) + (x76 * x69);
	const FLT x83 = (-1 * x82 * sensor_pt[2]) + (x80 * sensor_pt[1]) + (x75 * sensor_pt[0]);
	const FLT x84 = x72 * x40;
	const FLT x85 = (-1 * x77 * x47) + (x79 * x33) + (x70 * x37) + x84;
	const FLT x86 = (-1 * x85 * sensor_pt[0]) + (x80 * sensor_pt[2]) + (x82 * sensor_pt[1]);
	const FLT x87 = (*_x0).Object.Pose.Pos[0] + (x32 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (2 * ((x85 * x86) + (-1 * x83 * x75))) + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0]));
	const FLT x88 = x87 * x21;
	const FLT x89 = (x80 * sensor_pt[0]) + (-1 * x75 * sensor_pt[1]) + (x85 * sensor_pt[2]);
	const FLT x90 = (*_x0).Object.Pose.Pos[2] + (2 * ((x82 * x83) + (-1 * x89 * x85))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x32 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x91 = x90 * x21;
	const FLT x92 = (x32 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*_x0).Object.Pose.Pos[1] + (2 * ((x89 * x75) + (-1 * x82 * x86))) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1]));
	const FLT x93 = x92 * x21;
	const FLT x94 = (x6 * x93) + (x8 * x88) + (-1 * x4 * x91);
	const FLT x95 = (x91 * x11) + (x8 * x93) + (-1 * x6 * x88);
	const FLT x96 = x90 + (-1 * (x5 + (2 * ((-1 * x30 * x29) + (x28 * x27))))) + (2 * ((-1 * x95 * x30) + (x94 * x28)));
	const FLT x97 = x6 * x21;
	const FLT x98 = (x4 * x24) + (x8 * x22) + (-1 * x26 * x11);
	const FLT x99 = (x4 * x88) + (x8 * x91) + (-1 * x93 * x11);
	const FLT x100 = x99 * x21;
	const FLT x101 = x87 + (-1 * (x23 + (2 * ((-1 * x98 * x28) + (x97 * x29))))) + (2 * ((-1 * x4 * x100) + (x97 * x95)));
	const FLT x102 = atan2(-1 * x96, x101);
	const FLT x103 = 0.523598775598299 + (*error_model).BSD0.tilt + (*_x0).BSD0.tilt;
	const FLT x104 = tan(x103);
	const FLT x105 = x92 + (2 * ((-1 * x97 * x94) + (x11 * x100))) + (-1 * (x25 + (2 * ((-1 * x97 * x27) + (x98 * x30)))));
	const FLT x106 = x101 * x101;
	const FLT x107 = x106 + (x96 * x96);
	const FLT x108 = 1. / sqrt(x107);
	const FLT x109 = x108 * x105;
	const FLT x110 = x109 * x104;
	const FLT x111 = asin(x110) + (-1 * x102) + (-1 * (*error_model).BSD0.ogeephase) + (-1 * (*_x0).BSD0.ogeephase);
	const FLT x112 = sin(x111);
	const FLT x113 = (*_x0).BSD0.curve + (-1 * x0 * x112) + (*error_model).BSD0.curve;
	const FLT x114 = x105 * x105;
	const FLT x115 = x107 + x114;
	const FLT x116 = 1. / sqrt(x115);
	const FLT x117 = cos(x103);
	const FLT x118 = 1. / x117;
	const FLT x119 = x118 * x116;
	const FLT x120 = asin(x105 * x119);
	const FLT x121 = 8.0108022e-06 * x120;
	const FLT x122 = -8.0108022e-06 + (-1 * x121);
	const FLT x123 = 0.0028679863 + (x120 * x122);
	const FLT x124 = 5.3685255e-06 + (x120 * x123);
	const FLT x125 = 0.0076069798 + (x120 * x124);
	const FLT x126 = x120 * x125;
	const FLT x127 = -8.0108022e-06 + (-1.60216044e-05 * x120);
	const FLT x128 = x123 + (x120 * x127);
	const FLT x129 = x124 + (x120 * x128);
	const FLT x130 = x125 + (x120 * x129);
	const FLT x131 = (x120 * x130) + x126;
	const FLT x132 = sin(x103);
	const FLT x133 = x113 * x132;
	const FLT x134 = x133 * x131;
	const FLT x135 = x117 + (-1 * x134);
	const FLT x136 = 1. / x135;
	const FLT x137 = x120 * x120;
	const FLT x138 = x137 * x136;
	const FLT x139 = x125 * x138;
	const FLT x140 = x110 + (x113 * x139);
	const FLT x141 = 1. / sqrt(1 + (-1 * (x140 * x140)));
	const FLT x142 = 1. / x18;
	const FLT x143 = 2 * x6;
	const FLT x144 = x14 * x143;
	const FLT x145 = x142 * x144;
	const FLT x146 = x8 * x145;
	const FLT x147 = 2 * x4;
	const FLT x148 = x14 * x147;
	const FLT x149 = x142 * x148;
	const FLT x150 = x11 * x149;
	const FLT x151 = -1 * x150;
	const FLT x152 = x151 + x146;
	const FLT x153 = 2 * x105;
	const FLT x154 = 2 * x142;
	const FLT x155 = x16 * x154;
	const FLT x156 = x15 * x154;
	const FLT x157 = -1 + x156 + x155;
	const FLT x158 = 2 * x101;
	const FLT x159 = x11 * x145;
	const FLT x160 = -1 * x159;
	const FLT x161 = x8 * x149;
	const FLT x162 = -1 * x161;
	const FLT x163 = x162 + x160;
	const FLT x164 = 2 * x96;
	const FLT x165 = (x164 * x163) + (x157 * x158);
	const FLT x166 = 1.0/2.0 * x105;
	const FLT x167 = x118 * (1. / (x115 * sqrt(x115))) * x166;
	const FLT x168 = (x119 * x152) + (-1 * x167 * (x165 + (x152 * x153)));
	const FLT x169 = 1. / (x117 * x117);
	const FLT x170 = 1. / sqrt(1 + (-1 * x114 * (1. / x115) * x169));
	const FLT x171 = x126 * x170 * x136;
	const FLT x172 = 2 * x113;
	const FLT x173 = x171 * x172;
	const FLT x174 = x123 * x170;
	const FLT x175 = x121 * x170;
	const FLT x176 = x122 * x170;
	const FLT x177 = x168 * x176;
	const FLT x178 = (x120 * (x177 + (-1 * x168 * x175))) + (x168 * x174);
	const FLT x179 = x124 * x170;
	const FLT x180 = (x168 * x179) + (x120 * x178);
	const FLT x181 = x113 * x138;
	const FLT x182 = x170 * x130;
	const FLT x183 = x129 * x170;
	const FLT x184 = x127 * x170;
	const FLT x185 = 2.40324066e-05 * x120;
	const FLT x186 = x170 * x185;
	const FLT x187 = x128 * x170;
	const FLT x188 = x125 * x170;
	const FLT x189 = 1. / x107;
	const FLT x190 = x104 * x104;
	const FLT x191 = 1. / sqrt(1 + (-1 * x114 * x189 * x190));
	const FLT x192 = x104 * (1. / (x107 * sqrt(x107))) * x166;
	const FLT x193 = x108 * x104;
	const FLT x194 = (x193 * x152) + (-1 * x165 * x192);
	const FLT x195 = 1. / x101;
	const FLT x196 = x96 * (1. / x106);
	const FLT x197 = x106 * x189;
	const FLT x198 = ((x196 * x157) + (-1 * x163 * x195)) * x197;
	const FLT x199 = (-1 * x198) + (x191 * x194);
	const FLT x200 = x0 * cos(x111);
	const FLT x201 = x131 * x132;
	const FLT x202 = x200 * x201;
	const FLT x203 = x125 * x137 * (1. / (x135 * x135));
	const FLT x204 = x203 * x113;
	const FLT x205 = x200 * x139;
	const FLT x206 = x198 + (-1 * x141 * (x194 + (-1 * x205 * x199) + (x168 * x173) + (-1 * x204 * ((x202 * x199) + (-1 * x133 * ((x168 * x188) + (x168 * x182) + (x120 * x180) + (x120 * (x180 + (x168 * x183) + (x120 * (x178 + (x120 * ((-1 * x168 * x186) + x177 + (x168 * x184))) + (x168 * x187))))))))) + (x181 * x180)));
	const FLT x207 = (-1 * asin(x140)) + x102 + (*error_model).BSD0.gibpha + (*_x0).BSD0.gibpha;
	const FLT x208 = cos(x207) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag);
	const FLT x209 = -1 * x146;
	const FLT x210 = x209 + x151;
	const FLT x211 = 2 * x8;
	const FLT x212 = x14 * x211;
	const FLT x213 = x11 * x212 * x142;
	const FLT x214 = x4 * x145;
	const FLT x215 = -1 * x214;
	const FLT x216 = x215 + x213;
	const FLT x217 = (x216 * x164) + (x210 * x158);
	const FLT x218 = x17 * x154;
	const FLT x219 = -1 + x218;
	const FLT x220 = x219 + x156;
	const FLT x221 = (x220 * x193) + (-1 * x217 * x192);
	const FLT x222 = ((x210 * x196) + (-1 * x216 * x195)) * x197;
	const FLT x223 = (-1 * x222) + (x221 * x191);
	const FLT x224 = (x220 * x119) + (-1 * x167 * (x217 + (x220 * x153)));
	const FLT x225 = x224 * x176;
	const FLT x226 = (x120 * (x225 + (-1 * x224 * x175))) + (x224 * x174);
	const FLT x227 = (x224 * x179) + (x226 * x120);
	const FLT x228 = x222 + (-1 * x141 * (x221 + (-1 * x205 * x223) + (-1 * x204 * ((x202 * x223) + (-1 * x133 * ((x227 * x120) + (x224 * x182) + (x120 * (x227 + (x224 * x183) + (x120 * (x226 + (x120 * ((-1 * x224 * x186) + x225 + (x224 * x184))) + (x224 * x187))))) + (x224 * x188))))) + (x224 * x173) + (x227 * x181)));
	const FLT x229 = -1 * x213;
	const FLT x230 = x229 + x215;
	const FLT x231 = x160 + x161;
	const FLT x232 = x219 + x155;
	const FLT x233 = (x232 * x164) + (x231 * x158);
	const FLT x234 = (x230 * x119) + (-1 * x167 * (x233 + (x230 * x153)));
	const FLT x235 = x234 * x176;
	const FLT x236 = (x120 * (x235 + (-1 * x234 * x175))) + (x234 * x174);
	const FLT x237 = (x234 * x179) + (x236 * x120);
	const FLT x238 = (x230 * x193) + (-1 * x233 * x192);
	const FLT x239 = ((x231 * x196) + (-1 * x232 * x195)) * x197;
	const FLT x240 = (-1 * x239) + (x238 * x191);
	const FLT x241 = x239 + (-1 * x141 * (x238 + (-1 * x204 * ((x202 * x240) + (-1 * x133 * ((x237 * x120) + (x120 * (x237 + (x234 * x183) + (x120 * (x236 + (x120 * (x235 + (-1 * x234 * x186) + (x234 * x184))) + (x234 * x187))))) + (x234 * x182) + (x234 * x188))))) + (-1 * x205 * x240) + (x237 * x181) + (x234 * x173)));
	const FLT x242 = 1.0 * x4;
	const FLT x243 = x242 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x244 = 1.0 * x11;
	const FLT x245 = x244 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x246 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x247 = x6 * x246;
	const FLT x248 = x211 + x247 + x243 + x245;
	const FLT x249 = 1. / (x13 * x13);
	const FLT x250 = x9 * x249;
	const FLT x251 = x7 * x249;
	const FLT x252 = x12 * x249;
	const FLT x253 = x10 * x249;
	const FLT x254 = x20 * (1. / (x18 * sqrt(x18)));
	const FLT x255 = x254 * (x212 + (-1 * x253 * x248) + (-1 * x251 * x248) + (-1 * x250 * x248) + (-1 * x252 * x248) + (x14 * x245) + (x14 * x243) + (x14 * x247));
	const FLT x256 = 1.0/2.0 * x90;
	const FLT x257 = x11 * x256;
	const FLT x258 = x1 * x91;
	const FLT x259 = (1. / (x13 * sqrt(x13))) * x19;
	const FLT x260 = x259 * x248;
	const FLT x261 = x6 * x260;
	const FLT x262 = 1.0/2.0 * x87;
	const FLT x263 = 1.0/2.0 * x8;
	const FLT x264 = x92 * x263;
	const FLT x265 = -1 * x3 * x88;
	const FLT x266 = x6 * x255;
	const FLT x267 = (-1 * x264 * x260) + (-1 * x260 * x257) + x93 + (x266 * x262) + x258 + (x262 * x261) + x265 + (-1 * x255 * x257) + (-1 * x264 * x255);
	const FLT x268 = x21 * x143;
	const FLT x269 = x95 * x21;
	const FLT x270 = x269 * x246;
	const FLT x271 = x6 * x95;
	const FLT x272 = x21 * x29;
	const FLT x273 = x272 * x246;
	const FLT x274 = 1.0/2.0 * x23;
	const FLT x275 = 1.0/2.0 * x11;
	const FLT x276 = x5 * x275;
	const FLT x277 = x263 * x260;
	const FLT x278 = x1 * x22;
	const FLT x279 = x263 * x255;
	const FLT x280 = -1 * x3 * x24;
	const FLT x281 = x278 + (-1 * x276 * x255) + (-1 * x25 * x279) + (x274 * x261) + x26 + (x274 * x266) + (-1 * x276 * x260) + x280 + (-1 * x25 * x277);
	const FLT x282 = x4 * x99;
	const FLT x283 = x4 * x274;
	const FLT x284 = x25 * x275;
	const FLT x285 = x2 * x24;
	const FLT x286 = -1 * x1 * x26;
	const FLT x287 = x285 + (x284 * x255) + (-1 * x5 * x277) + (-1 * x283 * x255) + (-1 * x260 * x283) + x22 + x286 + (x260 * x284) + (-1 * x5 * x279);
	const FLT x288 = x21 * x147;
	const FLT x289 = x4 * x98;
	const FLT x290 = x4 * x262;
	const FLT x291 = x92 * x275;
	const FLT x292 = x8 * x256;
	const FLT x293 = x2 * x88;
	const FLT x294 = -1 * x1 * x93;
	const FLT x295 = x294 + x91;
	const FLT x296 = x293 + (-1 * x292 * x260) + (-1 * x290 * x255) + (-1 * x290 * x260) + (x291 * x255) + (x291 * x260) + x295 + (-1 * x292 * x255);
	const FLT x297 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x298 = x98 * x21;
	const FLT x299 = (x297 * x298) + (-1 * x297 * x100);
	const FLT x300 = (x282 * x255) + (-1 * x296 * x288) + x299 + (-1 * x260 * x289) + (-1 * x271 * x260) + (x268 * x267) + (x29 * x261) + x270 + (x260 * x282) + (-1 * x271 * x255) + (-1 * x268 * x281) + (-1 * x273) + (x29 * x266) + (x287 * x288) + (-1 * x289 * x255);
	const FLT x301 = x95 * x11;
	const FLT x302 = x4 * x94;
	const FLT x303 = x29 * x11;
	const FLT x304 = x4 * x27;
	const FLT x305 = x94 * x21;
	const FLT x306 = x297 * x305;
	const FLT x307 = x21 * x27;
	const FLT x308 = x297 * x307;
	const FLT x309 = 2 * x11;
	const FLT x310 = x21 * x309;
	const FLT x311 = 1.0/2.0 * x25;
	const FLT x312 = x3 * x26;
	const FLT x313 = x8 * x274;
	const FLT x314 = 1.0/2.0 * x5;
	const FLT x315 = x4 * x314;
	const FLT x316 = -1 * x2 * x22;
	const FLT x317 = (-1 * x255 * x313) + x24 + x312 + x316 + (-1 * x261 * x311) + (x255 * x315) + (-1 * x260 * x313) + (x260 * x315) + (-1 * x266 * x311);
	const FLT x318 = 1.0/2.0 * x92;
	const FLT x319 = x3 * x93;
	const FLT x320 = x87 * x263;
	const FLT x321 = x4 * x256;
	const FLT x322 = -1 * x2 * x91;
	const FLT x323 = x88 + x322;
	const FLT x324 = (-1 * x261 * x318) + x323 + x319 + (-1 * x260 * x320) + (-1 * x266 * x318) + (x255 * x321) + (x260 * x321) + (-1 * x255 * x320);
	const FLT x325 = 1.0 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x326 = (x272 * x325) + (-1 * x269 * x325);
	const FLT x327 = x326 + (-1 * x288 * x317) + (-1 * x260 * x303) + (-1 * x308) + (x260 * x304) + (x255 * x301) + (-1 * x255 * x302) + (x260 * x301) + (-1 * x255 * x303) + (x281 * x310) + (-1 * x260 * x302) + x306 + (-1 * x267 * x310) + (x288 * x324) + (x255 * x304);
	const FLT x328 = (x327 * x164) + (x300 * x158);
	const FLT x329 = x98 * x11;
	const FLT x330 = x298 * x325;
	const FLT x331 = x99 * x11;
	const FLT x332 = x6 * x27;
	const FLT x333 = x325 * x100;
	const FLT x334 = (x246 * x307) + (-1 * x246 * x305);
	const FLT x335 = (-1 * x287 * x310) + (x260 * x329) + (-1 * x268 * x324) + (x255 * x329) + (x94 * x261) + x334 + (-1 * x27 * x261) + (-1 * x260 * x331) + (x268 * x317) + (-1 * x330) + (-1 * x255 * x331) + (-1 * x255 * x332) + (x296 * x310) + (x94 * x266) + x333;
	const FLT x336 = (x335 * x193) + (-1 * x328 * x192);
	const FLT x337 = ((x300 * x196) + (-1 * x327 * x195)) * x197;
	const FLT x338 = (-1 * x337) + (x336 * x191);
	const FLT x339 = (x335 * x119) + (-1 * x167 * (x328 + (x335 * x153)));
	const FLT x340 = x339 * x176;
	const FLT x341 = (x120 * (x340 + (-1 * x339 * x175))) + (x339 * x174);
	const FLT x342 = (x339 * x179) + (x341 * x120);
	const FLT x343 = x337 + (-1 * x141 * (x336 + (-1 * x204 * ((x202 * x338) + (-1 * x133 * ((x342 * x120) + (x339 * x182) + (x120 * (x342 + (x339 * x183) + (x120 * (x341 + (x120 * (x340 + (-1 * x339 * x186) + (x339 * x184))) + (x339 * x187))))) + (x339 * x188))))) + (x339 * x173) + (-1 * x205 * x338) + (x342 * x181)));
	const FLT x344 = x6 * x297;
	const FLT x345 = x242 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x346 = x8 * x325;
	const FLT x347 = (-1 * x346) + (-1 * x345) + x344 + x309;
	const FLT x348 = x259 * x347;
	const FLT x349 = x274 * x348;
	const FLT x350 = x275 * x348;
	const FLT x351 = ((-1 * x14 * x345) + (x14 * x309) + (-1 * x252 * x347) + (-1 * x14 * x346) + (-1 * x250 * x347) + (-1 * x253 * x347) + (x14 * x344) + (-1 * x251 * x347)) * x254;
	const FLT x352 = x263 * x351;
	const FLT x353 = x263 * x348;
	const FLT x354 = (-1 * x278) + x280;
	const FLT x355 = x354 + (-1 * x26) + (x284 * x351) + (-1 * x4 * x349) + (-1 * x5 * x352) + (-1 * x5 * x353) + (x25 * x350) + (-1 * x283 * x351);
	const FLT x356 = x271 * x259;
	const FLT x357 = x6 * x29;
	const FLT x358 = x6 * x351;
	const FLT x359 = (-1 * x285) + x286;
	const FLT x360 = (x274 * x358) + x22 + (-1 * x5 * x350) + (-1 * x25 * x352) + (-1 * x25 * x353) + x359 + (x6 * x349) + (-1 * x276 * x351);
	const FLT x361 = x99 * x351;
	const FLT x362 = -1 * x293;
	const FLT x363 = 1.0/2.0 * x348;
	const FLT x364 = x6 * x363;
	const FLT x365 = x6 * x262;
	const FLT x366 = (x365 * x351) + x295 + (-1 * x92 * x353) + (-1 * x257 * x351) + (x87 * x364) + x362 + (-1 * x257 * x348) + (-1 * x264 * x351);
	const FLT x367 = x4 * x363;
	const FLT x368 = x265 + (-1 * x258);
	const FLT x369 = (-1 * x93) + (x291 * x351) + (-1 * x290 * x351) + (x92 * x350) + x368 + (-1 * x87 * x367) + (-1 * x292 * x348) + (-1 * x292 * x351);
	const FLT x370 = (x246 * x100) + (-1 * x298 * x246);
	const FLT x371 = (-1 * x272 * x297) + (x297 * x269);
	const FLT x372 = (-1 * x288 * x369) + (x351 * x357) + (-1 * x289 * x348) + (-1 * x289 * x351) + (x357 * x348) + (-1 * x268 * x360) + x371 + (x268 * x366) + x370 + (x288 * x355) + (-1 * x356 * x347) + (-1 * x271 * x351) + (x4 * x361) + (x282 * x348);
	const FLT x373 = 2 * x21;
	const FLT x374 = x95 * x373;
	const FLT x375 = x29 * x373;
	const FLT x376 = x2 * x26;
	const FLT x377 = x3 * x22;
	const FLT x378 = x1 * x24;
	const FLT x379 = (-1 * x358 * x311) + (-1 * x351 * x313) + (-1 * x25 * x364) + (-1 * x378) + x376 + (-1 * x8 * x349) + (x5 * x367) + (x351 * x315) + x377;
	const FLT x380 = x2 * x93;
	const FLT x381 = x1 * x88;
	const FLT x382 = x6 * x318;
	const FLT x383 = x3 * x91;
	const FLT x384 = (-1 * x382 * x351) + (x351 * x321) + (x321 * x348) + x383 + x380 + (-1 * x381) + (-1 * x351 * x320) + (-1 * x382 * x348) + (-1 * x87 * x353);
	const FLT x385 = x259 * x301;
	const FLT x386 = x334 + (-1 * x351 * x302) + (x385 * x347) + (-1 * x302 * x348) + x375 + (x351 * x304) + (-1 * x374) + (-1 * x366 * x310) + (-1 * x303 * x348) + (x360 * x310) + (-1 * x288 * x379) + (-1 * x351 * x303) + (x351 * x301) + (x288 * x384) + (x304 * x348);
	const FLT x387 = (x386 * x164) + (x372 * x158);
	const FLT x388 = x6 * x94;
	const FLT x389 = x99 * x373;
	const FLT x390 = x98 * x373;
	const FLT x391 = (-1 * x351 * x332) + (-1 * x268 * x384) + (-1 * x306) + (x329 * x348) + (x388 * x351) + (x388 * x348) + (-1 * x348 * x332) + (x268 * x379) + (-1 * x355 * x310) + (x351 * x329) + (-1 * x11 * x361) + (-1 * x390) + x308 + (-1 * x348 * x331) + (x369 * x310) + x389;
	const FLT x392 = (x391 * x193) + (-1 * x387 * x192);
	const FLT x393 = ((x372 * x196) + (-1 * x386 * x195)) * x197;
	const FLT x394 = (-1 * x393) + (x392 * x191);
	const FLT x395 = (x391 * x119) + (-1 * x167 * (x387 + (x391 * x153)));
	const FLT x396 = x395 * x176;
	const FLT x397 = (x120 * (x396 + (-1 * x395 * x175))) + (x395 * x174);
	const FLT x398 = (x395 * x179) + (x397 * x120);
	const FLT x399 = x393 + (-1 * x141 * (x392 + (-1 * x204 * ((x202 * x394) + (-1 * x133 * ((x398 * x120) + (x395 * x188) + (x120 * (x398 + (x395 * x183) + (x120 * (x397 + (x120 * ((-1 * x395 * x186) + x396 + (x395 * x184))) + (x395 * x187))))) + (x395 * x182))))) + (x398 * x181) + (-1 * x205 * x394) + (x395 * x173)));
	const FLT x400 = x244 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x401 = x6 * x325;
	const FLT x402 = x8 * x297;
	const FLT x403 = (-1 * x402) + (-1 * x401) + x147 + x400;
	const FLT x404 = x403 * x259;
	const FLT x405 = x254 * ((-1 * x14 * x402) + x148 + (-1 * x403 * x251) + (x14 * x400) + (-1 * x403 * x250) + (-1 * x403 * x253) + (-1 * x403 * x252) + (-1 * x14 * x401));
	const FLT x406 = x4 * x405;
	const FLT x407 = x405 * x263;
	const FLT x408 = x404 * x263;
	const FLT x409 = (-1 * x312) + x316;
	const FLT x410 = x409 + (x405 * x284) + (-1 * x406 * x274) + (x404 * x284) + (-1 * x5 * x407) + (-1 * x5 * x408) + (-1 * x404 * x283) + x24;
	const FLT x411 = -1 * x319;
	const FLT x412 = (x405 * x291) + x411 + (x404 * x291) + x323 + (-1 * x405 * x292) + (-1 * x406 * x262) + (-1 * x404 * x290) + (-1 * x404 * x292);
	const FLT x413 = x6 * x404;
	const FLT x414 = x6 * x405;
	const FLT x415 = (x414 * x274) + x378 + (-1 * x376) + (-1 * x404 * x276) + (-1 * x25 * x408) + x377 + (-1 * x25 * x407) + (x413 * x274) + (-1 * x405 * x276);
	const FLT x416 = (x414 * x262) + x381 + (-1 * x404 * x264) + (-1 * x405 * x257) + (-1 * x405 * x264) + (-1 * x380) + x383 + (-1 * x404 * x257) + (x404 * x365);
	const FLT x417 = x326 + (x405 * x282) + (x416 * x268) + x390 + (-1 * x405 * x271) + (x410 * x288) + (-1 * x404 * x271) + (x404 * x282) + (-1 * x389) + (-1 * x404 * x289) + (x404 * x357) + (-1 * x405 * x289) + (-1 * x415 * x268) + (-1 * x412 * x288) + (x29 * x414);
	const FLT x418 = x94 * x373;
	const FLT x419 = x359 + (-1 * x413 * x311) + (-1 * x405 * x313) + (-1 * x404 * x313) + (-1 * x22) + (x406 * x314) + (-1 * x414 * x311) + (x404 * x315);
	const FLT x420 = x27 * x373;
	const FLT x421 = (x405 * x321) + (-1 * x91) + (-1 * x404 * x382) + (-1 * x404 * x320) + (-1 * x405 * x320) + (x404 * x321) + (-1 * x414 * x318) + x294 + x362;
	const FLT x422 = (x421 * x288) + (x405 * x301) + (x404 * x301) + (-1 * x420) + (x404 * x304) + (-1 * x405 * x302) + (-1 * x416 * x310) + x418 + (-1 * x404 * x303) + (-1 * x270) + (x405 * x304) + (-1 * x404 * x302) + (-1 * x419 * x288) + x273 + (-1 * x405 * x303) + (x415 * x310);
	const FLT x423 = (x422 * x164) + (x417 * x158);
	const FLT x424 = (-1 * x307 * x325) + (x305 * x325);
	const FLT x425 = x424 + x370 + (x404 * x329) + (x419 * x268) + (x412 * x310) + (-1 * x410 * x310) + (-1 * x404 * x331) + (-1 * x404 * x332) + (x404 * x388) + (-1 * x421 * x268) + (-1 * x405 * x331) + (x94 * x414) + (x405 * x329) + (-1 * x27 * x414);
	const FLT x426 = (x425 * x193) + (-1 * x423 * x192);
	const FLT x427 = ((x417 * x196) + (-1 * x422 * x195)) * x197;
	const FLT x428 = (-1 * x427) + (x426 * x191);
	const FLT x429 = (x425 * x119) + (-1 * x167 * (x423 + (x425 * x153)));
	const FLT x430 = x429 * x176;
	const FLT x431 = (x120 * (x430 + (-1 * x429 * x175))) + (x429 * x174);
	const FLT x432 = (x429 * x179) + (x431 * x120);
	const FLT x433 = x427 + (-1 * x141 * (x426 + (x429 * x173) + (-1 * x428 * x205) + (-1 * x204 * ((x428 * x202) + (-1 * x133 * ((x432 * x120) + (x429 * x182) + (x429 * x188) + (x120 * (x432 + (x429 * x183) + (x120 * (x431 + (x120 * ((-1 * x429 * x186) + x430 + (x429 * x184))) + (x429 * x187))))))))) + (x432 * x181)));
	const FLT x434 = x244 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x435 = x4 * x325;
	const FLT x436 = x8 * x246;
	const FLT x437 = (-1 * x436) + x143 + (-1 * x434) + x435;
	const FLT x438 = x437 * x259;
	const FLT x439 = x254 * ((-1 * x14 * x436) + (-1 * x437 * x251) + (-1 * x437 * x252) + (-1 * x437 * x250) + (-1 * x14 * x434) + (x14 * x435) + x144 + (-1 * x437 * x253));
	const FLT x440 = x92 * x439;
	const FLT x441 = x6 * x439;
	const FLT x442 = (x441 * x262) + (-1 * x88) + (-1 * x440 * x263) + (-1 * x439 * x257) + (-1 * x438 * x257) + x322 + (x438 * x365) + x411 + (-1 * x438 * x264);
	const FLT x443 = x439 * x263;
	const FLT x444 = x438 * x263;
	const FLT x445 = x6 * x438;
	const FLT x446 = (-1 * x24) + (-1 * x438 * x276) + (x445 * x274) + (-1 * x25 * x443) + x409 + (x441 * x274) + (-1 * x25 * x444) + (-1 * x439 * x276);
	const FLT x447 = x98 * x439;
	const FLT x448 = x376 + (-1 * x377) + (x439 * x284) + x378 + (-1 * x5 * x443) + (-1 * x439 * x283) + (-1 * x5 * x444) + (x438 * x284) + (-1 * x438 * x283);
	const FLT x449 = (x440 * x275) + (-1 * x383) + x380 + (-1 * x439 * x290) + (x438 * x291) + (-1 * x438 * x290) + x381 + (-1 * x439 * x292) + (-1 * x438 * x292);
	const FLT x450 = (-1 * x449 * x288) + (x439 * x282) + (-1 * x375) + x374 + (-1 * x439 * x271) + x330 + (-1 * x4 * x447) + (x442 * x268) + (-1 * x437 * x356) + (x29 * x441) + (x448 * x288) + (x438 * x282) + (-1 * x333) + (-1 * x446 * x268) + (x438 * x357) + (-1 * x438 * x289);
	const FLT x451 = x368 + (-1 * x441 * x318) + (-1 * x438 * x382) + (x439 * x321) + (-1 * x439 * x320) + x93 + (x438 * x321) + (-1 * x438 * x320);
	const FLT x452 = x354 + (-1 * x439 * x313) + (x438 * x315) + x26 + (-1 * x445 * x311) + (x439 * x315) + (-1 * x441 * x311) + (-1 * x438 * x313);
	const FLT x453 = x424 + (x439 * x301) + x371 + (x451 * x288) + (-1 * x442 * x310) + (-1 * x438 * x303) + (x446 * x310) + (x438 * x304) + (-1 * x438 * x302) + (x437 * x385) + (-1 * x452 * x288) + (-1 * x439 * x302) + (-1 * x439 * x303) + (x439 * x304);
	const FLT x454 = (x453 * x164) + (x450 * x158);
	const FLT x455 = x299 + x420 + (x438 * x388) + (-1 * x439 * x331) + (-1 * x418) + (-1 * x27 * x441) + (-1 * x438 * x332) + (x438 * x329) + (-1 * x438 * x331) + (x452 * x268) + (x449 * x310) + (x11 * x447) + (x94 * x441) + (-1 * x451 * x268) + (-1 * x448 * x310);
	const FLT x456 = (x455 * x193) + (-1 * x454 * x192);
	const FLT x457 = ((x450 * x196) + (-1 * x453 * x195)) * x197;
	const FLT x458 = (-1 * x457) + (x456 * x191);
	const FLT x459 = (x455 * x119) + (-1 * x167 * (x454 + (x455 * x153)));
	const FLT x460 = x459 * x176;
	const FLT x461 = (x120 * (x460 + (-1 * x459 * x175))) + (x459 * x174);
	const FLT x462 = (x459 * x179) + (x461 * x120);
	const FLT x463 = x457 + (-1 * x141 * ((-1 * x204 * ((x458 * x202) + (-1 * x133 * ((x462 * x120) + (x459 * x188) + (x459 * x182) + (x120 * (x462 + (x459 * x183) + (x120 * (x461 + (x120 * ((-1 * x459 * x186) + x460 + (x459 * x184))) + (x459 * x187))))))))) + (-1 * x458 * x205) + x456 + (x462 * x181) + (x459 * x173)));
	const FLT x464 = x31 * x142;
	const FLT x465 = -1 * x16 * x464;
	const FLT x466 = -1 * x15 * x464;
	const FLT x467 = x466 + x32 + x465;
	const FLT x468 = x14 * x464;
	const FLT x469 = x4 * x468;
	const FLT x470 = x8 * x469;
	const FLT x471 = x6 * x11 * x468;
	const FLT x472 = x471 + x470;
	const FLT x473 = (x472 * x164) + (x467 * x158);
	const FLT x474 = x11 * x469;
	const FLT x475 = x8 * x468;
	const FLT x476 = x6 * x475;
	const FLT x477 = (-1 * x476) + x474;
	const FLT x478 = (x477 * x193) + (-1 * x473 * x192);
	const FLT x479 = ((x467 * x196) + (-1 * x472 * x195)) * x197;
	const FLT x480 = (-1 * x479) + (x478 * x191);
	const FLT x481 = (x477 * x119) + (-1 * x167 * (x473 + (x477 * x153)));
	const FLT x482 = x481 * x176;
	const FLT x483 = (x120 * (x482 + (-1 * x481 * x175))) + (x481 * x174);
	const FLT x484 = (x481 * x179) + (x483 * x120);
	const FLT x485 = x479 + (-1 * x141 * ((-1 * x204 * ((x480 * x202) + (-1 * x133 * ((x484 * x120) + (x481 * x182) + (x481 * x188) + (x120 * (x484 + (x481 * x183) + (x120 * (x483 + (x120 * ((-1 * x481 * x186) + x482 + (x481 * x184))) + (x481 * x187))))))))) + x478 + (x484 * x181) + (-1 * x480 * x205) + (x481 * x173)));
	const FLT x486 = x474 + x476;
	const FLT x487 = x6 * x469;
	const FLT x488 = x11 * x475;
	const FLT x489 = (-1 * x488) + x487;
	const FLT x490 = (x489 * x164) + (x486 * x158);
	const FLT x491 = (-1 * x17 * x464) + x32;
	const FLT x492 = x491 + x466;
	const FLT x493 = (x492 * x193) + (-1 * x490 * x192);
	const FLT x494 = ((x486 * x196) + (-1 * x489 * x195)) * x197;
	const FLT x495 = (-1 * x494) + (x493 * x191);
	const FLT x496 = (x492 * x119) + (-1 * x167 * (x490 + (x492 * x153)));
	const FLT x497 = x496 * x176;
	const FLT x498 = (x120 * (x497 + (-1 * x496 * x175))) + (x496 * x174);
	const FLT x499 = (x496 * x179) + (x498 * x120);
	const FLT x500 = x494 + (-1 * x141 * ((-1 * x204 * ((x495 * x202) + (-1 * x133 * ((x499 * x120) + (x120 * (x499 + (x496 * x183) + (x120 * (x498 + (x120 * ((-1 * x496 * x186) + x497 + (x496 * x184))) + (x496 * x187))))) + (x496 * x188) + (x496 * x182))))) + x493 + (x499 * x181) + (-1 * x495 * x205) + (x496 * x173)));
	const FLT x501 = x487 + x488;
	const FLT x502 = (-1 * x470) + x471;
	const FLT x503 = x491 + x465;
	const FLT x504 = (x503 * x164) + (x502 * x158);
	const FLT x505 = (x501 * x119) + (-1 * x167 * (x504 + (x501 * x153)));
	const FLT x506 = (x501 * x193) + (-1 * x504 * x192);
	const FLT x507 = ((x502 * x196) + (-1 * x503 * x195)) * x197;
	const FLT x508 = (-1 * x507) + (x506 * x191);
	const FLT x509 = x505 * x176;
	const FLT x510 = (x120 * (x509 + (-1 * x505 * x175))) + (x505 * x174);
	const FLT x511 = (x505 * x179) + (x510 * x120);
	const FLT x512 = x507 + (-1 * x141 * (x506 + (x511 * x181) + (x505 * x173) + (-1 * x204 * ((x508 * x202) + (-1 * x133 * ((x511 * x120) + (x120 * (x511 + (x505 * x183) + (x120 * ((x120 * (x509 + (-1 * x505 * x186) + (x505 * x184))) + x510 + (x505 * x187))))) + (x505 * x182) + (x505 * x188))))) + (-1 * x508 * x205)));
	const FLT x513 = -1 * x155;
	const FLT x514 = 1 + (-1 * x156);
	const FLT x515 = x514 + x513;
	const FLT x516 = x159 + x161;
	const FLT x517 = (x516 * x164) + (x515 * x158);
	const FLT x518 = x209 + x150;
	const FLT x519 = (x518 * x193) + (-1 * x517 * x192);
	const FLT x520 = ((x515 * x196) + (-1 * x516 * x195)) * x197;
	const FLT x521 = (-1 * x520) + (x519 * x191);
	const FLT x522 = (x518 * x119) + (-1 * x167 * (x517 + (x518 * x153)));
	const FLT x523 = x522 * x176;
	const FLT x524 = (x120 * (x523 + (-1 * x522 * x175))) + (x522 * x174);
	const FLT x525 = (x522 * x179) + (x524 * x120);
	const FLT x526 = x520 + (-1 * x141 * ((x525 * x181) + x519 + (-1 * x204 * ((x521 * x202) + (-1 * x133 * ((x525 * x120) + (x522 * x188) + (x522 * x182) + (x120 * (x525 + (x522 * x183) + (x120 * (x524 + (x120 * ((-1 * x522 * x186) + x523 + (x522 * x184))) + (x522 * x187))))))))) + (-1 * x521 * x205) + (x522 * x173)));
	const FLT x527 = x150 + x146;
	const FLT x528 = x229 + x214;
	const FLT x529 = (x528 * x164) + (x527 * x158);
	const FLT x530 = -1 * x218;
	const FLT x531 = x514 + x530;
	const FLT x532 = (x531 * x193) + (-1 * x529 * x192);
	const FLT x533 = ((x527 * x196) + (-1 * x528 * x195)) * x197;
	const FLT x534 = (-1 * x533) + (x532 * x191);
	const FLT x535 = (x531 * x119) + (-1 * x167 * (x529 + (x531 * x153)));
	const FLT x536 = x535 * x176;
	const FLT x537 = (x120 * (x536 + (-1 * x535 * x175))) + (x535 * x174);
	const FLT x538 = (x535 * x179) + (x537 * x120);
	const FLT x539 = x533 + (-1 * x141 * ((x538 * x181) + (-1 * x534 * x205) + x532 + (x535 * x173) + (-1 * x204 * ((x534 * x202) + (-1 * x133 * ((x538 * x120) + (x535 * x188) + (x535 * x182) + (x120 * ((x535 * x183) + x538 + (x120 * (x537 + (x120 * ((-1 * x535 * x186) + (x535 * x184) + x536)) + (x535 * x187)))))))))));
	const FLT x540 = x162 + x159;
	const FLT x541 = 1 + x530 + x513;
	const FLT x542 = (x541 * x164) + (x540 * x158);
	const FLT x543 = x214 + x213;
	const FLT x544 = (x543 * x193) + (-1 * x542 * x192);
	const FLT x545 = ((x540 * x196) + (-1 * x541 * x195)) * x197;
	const FLT x546 = (-1 * x545) + (x544 * x191);
	const FLT x547 = (x543 * x119) + (-1 * x167 * (x542 + (x543 * x153)));
	const FLT x548 = x547 * x176;
	const FLT x549 = (x120 * (x548 + (-1 * x547 * x175))) + (x547 * x174);
	const FLT x550 = (x547 * x179) + (x549 * x120);
	const FLT x551 = x545 + (-1 * x141 * (x544 + (-1 * x204 * ((x546 * x202) + (-1 * x133 * ((x547 * x188) + (x550 * x120) + (x120 * (x550 + (x547 * x183) + (x120 * (x549 + (x120 * ((x547 * x184) + (-1 * x547 * x186) + x548)) + (x547 * x187))))) + (x547 * x182))))) + (x550 * x181) + (-1 * x546 * x205) + (x547 * x173)));
	const FLT x552 = 1.0 * x41;
	const FLT x553 = 1.0 * x39;
	const FLT x554 = 1.0 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x555 = (x40 * x554) + (x552 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x553 * (*error_model).Object.Pose.AxisAngleRot[2]) + (2 * x37);
	const FLT x556 = x65 * x47;
	const FLT x557 = 1.0/2.0 * x40;
	const FLT x558 = 1. / (x42 * sqrt(x42));
	const FLT x559 = x62 * x558;
	const FLT x560 = x559 * x557;
	const FLT x561 = x556 * x560;
	const FLT x562 = x71 * x558;
	const FLT x563 = x555 * x562;
	const FLT x564 = 1.0/2.0 * x563;
	const FLT x565 = 1.0/2.0 * x37;
	const FLT x566 = x65 * x565;
	const FLT x567 = x33 * x566;
	const FLT x568 = x559 * x555;
	const FLT x569 = x66 * x33;
	const FLT x570 = x66 * x47;
	const FLT x571 = 0.5 * x570;
	const FLT x572 = x571 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x573 = 1.0/2.0 * x41;
	const FLT x574 = x69 * x573;
	const FLT x575 = 0.5 * x70;
	const FLT x576 = -1 * x575 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x577 = 0.5 * x72;
	const FLT x578 = x577 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x579 = x578 + x576;
	const FLT x580 = (-1 * x39 * x564) + (-1 * x567 * x568) + x579 + (-1 * x555 * x561) + (x574 * x568) + x569 + x572;
	const FLT x581 = 2 * x83;
	const FLT x582 = 1.0/2.0 * x39;
	const FLT x583 = x582 * x556;
	const FLT x584 = x65 * x33;
	const FLT x585 = x584 * x555;
	const FLT x586 = x585 * x559;
	const FLT x587 = x69 * x565;
	const FLT x588 = 0.5 * x569;
	const FLT x589 = x588 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x590 = -1 * x571 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x591 = x577 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x592 = x591 + x590;
	const FLT x593 = (-1 * x573 * x586) + (x583 * x568) + x592 + x70 + x589 + (-1 * x557 * x563) + (-1 * x587 * x568);
	const FLT x594 = x588 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x595 = -1 * x594;
	const FLT x596 = x575 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x597 = -1 * x596;
	const FLT x598 = x573 * x556;
	const FLT x599 = x69 * x560;
	const FLT x600 = x571 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x601 = (-1 * x600) + x72;
	const FLT x602 = (x599 * x555) + x597 + (x598 * x568) + x601 + x595 + (-1 * x37 * x564) + (x582 * x586);
	const FLT x603 = x47 * x566;
	const FLT x604 = x69 * x582;
	const FLT x605 = x577 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x606 = x575 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x607 = -1 * x588 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x608 = x607 + x606;
	const FLT x609 = x608 + x605 + x570 + (-1 * x603 * x568) + (-1 * x41 * x564) + (x585 * x560) + (-1 * x604 * x568);
	const FLT x610 = (x609 * sensor_pt[1]) + (-1 * x593 * sensor_pt[0]) + (x602 * sensor_pt[2]);
	const FLT x611 = 2 * x85;
	const FLT x612 = (x580 * sensor_pt[0]) + (-1 * x609 * sensor_pt[2]) + (x602 * sensor_pt[1]);
	const FLT x613 = 2 * x75;
	const FLT x614 = 2 * x86;
	const FLT x615 = (-1 * x612 * x613) + (-1 * x580 * x581) + (x614 * x593) + (x610 * x611);
	const FLT x616 = 2 * x89;
	const FLT x617 = (x593 * sensor_pt[2]) + (x602 * sensor_pt[0]) + (-1 * x580 * sensor_pt[1]);
	const FLT x618 = 2 * x82;
	const FLT x619 = (x609 * x581) + (-1 * x616 * x593) + (x612 * x618) + (-1 * x611 * x617);
	const FLT x620 = x8 * x21;
	const FLT x621 = (x613 * x617) + (x616 * x580) + (-1 * x614 * x609) + (-1 * x610 * x618);
	const FLT x622 = x21 * x621;
	const FLT x623 = (-1 * x11 * x622) + (x28 * x615) + (x619 * x620);
	const FLT x624 = (-1 * x28 * x619) + (x615 * x620) + (x6 * x622);
	const FLT x625 = x621 + (x623 * x310) + (-1 * x624 * x268);
	const FLT x626 = (-1 * x97 * x615) + (x30 * x619) + (x8 * x622);
	const FLT x627 = x615 + (-1 * x623 * x288) + (x626 * x268);
	const FLT x628 = x619 + (x624 * x288) + (-1 * x626 * x310);
	const FLT x629 = (x628 * x164) + (x627 * x158);
	const FLT x630 = (x625 * x119) + (-1 * x167 * (x629 + (x625 * x153)));
	const FLT x631 = x630 * x176;
	const FLT x632 = (x120 * (x631 + (-1 * x630 * x175))) + (x630 * x174);
	const FLT x633 = (x630 * x179) + (x632 * x120);
	const FLT x634 = (x625 * x193) + (-1 * x629 * x192);
	const FLT x635 = ((x627 * x196) + (-1 * x628 * x195)) * x197;
	const FLT x636 = (-1 * x635) + (x634 * x191);
	const FLT x637 = x635 + (-1 * x141 * (x634 + (-1 * x204 * ((x636 * x202) + (-1 * x133 * ((x633 * x120) + (x120 * (x633 + (x630 * x183) + (x120 * ((x120 * ((-1 * x630 * x186) + x631 + (x630 * x184))) + x632 + (x630 * x187))))) + (x630 * x182) + (x630 * x188))))) + (x630 * x173) + (x633 * x181) + (-1 * x636 * x205)));
	const FLT x638 = -1 * x572;
	const FLT x639 = 1.0 * x37;
	const FLT x640 = 1.0 * x40;
	const FLT x641 = (-1 * x640 * (*error_model).Object.Pose.AxisAngleRot[2]) + (2 * x41) + (-1 * x639 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x39 * x554);
	const FLT x642 = x641 * x559;
	const FLT x643 = x69 * x642;
	const FLT x644 = x573 * x584;
	const FLT x645 = x641 * x562;
	const FLT x646 = (-1 * x578) + x576;
	const FLT x647 = x646 + x569 + (-1 * x645 * x557) + x638 + (-1 * x642 * x644) + (x642 * x583) + (-1 * x643 * x565);
	const FLT x648 = -1 * x589;
	const FLT x649 = -1 * x70;
	const FLT x650 = (-1 * x645 * x582) + (-1 * x641 * x561) + x592 + (-1 * x642 * x567) + x649 + x648 + (x643 * x573);
	const FLT x651 = -1 * x570;
	const FLT x652 = x582 * x584;
	const FLT x653 = -1 * x605;
	const FLT x654 = x608 + x653 + (x641 * x599) + x651 + (x642 * x598) + (-1 * x645 * x565) + (x642 * x652);
	const FLT x655 = (x647 * sensor_pt[2]) + (-1 * x650 * sensor_pt[1]) + (x654 * sensor_pt[0]);
	const FLT x656 = x584 * x560;
	const FLT x657 = (x641 * x656) + x594 + (-1 * x645 * x573) + x596 + x601 + (-1 * x603 * x642) + (-1 * x643 * x582);
	const FLT x658 = (x654 * sensor_pt[1]) + (x650 * sensor_pt[0]) + (-1 * x657 * sensor_pt[2]);
	const FLT x659 = (x618 * x658) + (x657 * x581) + (-1 * x616 * x647) + (-1 * x611 * x655);
	const FLT x660 = x21 * x659;
	const FLT x661 = (x657 * sensor_pt[1]) + (-1 * x647 * sensor_pt[0]) + (x654 * sensor_pt[2]);
	const FLT x662 = (x616 * x650) + (-1 * x614 * x657) + (x613 * x655) + (-1 * x618 * x661);
	const FLT x663 = x21 * x662;
	const FLT x664 = (-1 * x613 * x658) + (-1 * x650 * x581) + (x614 * x647) + (x611 * x661);
	const FLT x665 = (-1 * x97 * x664) + (x11 * x660) + (x8 * x663);
	const FLT x666 = (-1 * x11 * x663) + (x28 * x664) + (x8 * x660);
	const FLT x667 = x664 + (x665 * x268) + (-1 * x666 * x288);
	const FLT x668 = (-1 * x4 * x660) + (x6 * x663) + (x620 * x664);
	const FLT x669 = x659 + (x668 * x288) + (-1 * x665 * x310);
	const FLT x670 = (x669 * x164) + (x667 * x158);
	const FLT x671 = x662 + (-1 * x668 * x268) + (x666 * x310);
	const FLT x672 = (x671 * x193) + (-1 * x670 * x192);
	const FLT x673 = ((x667 * x196) + (-1 * x669 * x195)) * x197;
	const FLT x674 = (-1 * x673) + (x672 * x191);
	const FLT x675 = (x671 * x119) + (-1 * x167 * (x670 + (x671 * x153)));
	const FLT x676 = x675 * x176;
	const FLT x677 = (x120 * (x676 + (-1 * x675 * x175))) + (x675 * x174);
	const FLT x678 = (x675 * x179) + (x677 * x120);
	const FLT x679 = x673 + (-1 * x141 * ((-1 * x204 * ((x674 * x202) + (-1 * x133 * ((x678 * x120) + (x675 * x182) + (x120 * ((x675 * x183) + x678 + (x120 * (x677 + (x120 * ((-1 * x675 * x186) + x676 + (x675 * x184))) + (x675 * x187))))) + (x675 * x188))))) + (x675 * x173) + (-1 * x674 * x205) + x672 + (x678 * x181)));
	const FLT x680 = (x552 * (*error_model).Object.Pose.AxisAngleRot[2]) + (2 * x40) + (-1 * x37 * x554) + (-1 * x553 * (*error_model).Object.Pose.AxisAngleRot[0]);
	const FLT x681 = x680 * x559;
	const FLT x682 = x69 * x681;
	const FLT x683 = x680 * x562;
	const FLT x684 = x72 + x600;
	const FLT x685 = x684 + (-1 * x644 * x681) + x594 + (x681 * x583) + (-1 * x683 * x557) + (-1 * x682 * x565) + x597;
	const FLT x686 = -1 * x569;
	const FLT x687 = x579 + (-1 * x604 * x681) + x638 + x686 + (-1 * x603 * x681) + (-1 * x683 * x573) + (x680 * x656);
	const FLT x688 = (-1 * x606) + x607;
	const FLT x689 = x688 + x570 + x653 + (-1 * x681 * x567) + (-1 * x683 * x582) + (x682 * x573) + (-1 * x680 * x561);
	const FLT x690 = (-1 * x591) + x590;
	const FLT x691 = x690 + x649 + (x681 * x598) + x589 + (-1 * x683 * x565) + (x681 * x652) + (x680 * x599);
	const FLT x692 = 2 * ((x685 * sensor_pt[2]) + (-1 * x689 * sensor_pt[1]) + (x691 * sensor_pt[0]));
	const FLT x693 = (x689 * sensor_pt[0]) + (-1 * x687 * sensor_pt[2]) + (x691 * sensor_pt[1]);
	const FLT x694 = (-1 * x85 * x692) + (x693 * x618) + (-1 * x616 * x685) + (x687 * x581);
	const FLT x695 = (x687 * sensor_pt[1]) + (-1 * x685 * sensor_pt[0]) + (x691 * sensor_pt[2]);
	const FLT x696 = (-1 * x695 * x618) + (x616 * x689) + (-1 * x614 * x687) + (x75 * x692);
	const FLT x697 = x21 * x696;
	const FLT x698 = (-1 * x693 * x613) + (x614 * x685) + (-1 * x689 * x581) + (x695 * x611);
	const FLT x699 = x21 * ((-1 * x97 * x698) + (x30 * x694) + (x8 * x697));
	const FLT x700 = (-1 * x11 * x697) + (x28 * x698) + (x694 * x620);
	const FLT x701 = x698 + (x699 * x143) + (-1 * x700 * x288);
	const FLT x702 = (-1 * x28 * x694) + (x6 * x697) + (x698 * x620);
	const FLT x703 = (x702 * x288) + x694 + (-1 * x699 * x309);
	const FLT x704 = (x703 * x164) + (x701 * x158);
	const FLT x705 = x696 + (x700 * x310) + (-1 * x702 * x268);
	const FLT x706 = (x705 * x193) + (-1 * x704 * x192);
	const FLT x707 = ((x701 * x196) + (-1 * x703 * x195)) * x197;
	const FLT x708 = (-1 * x707) + (x706 * x191);
	const FLT x709 = (x705 * x119) + (-1 * x167 * (x704 + (x705 * x153)));
	const FLT x710 = x709 * x176;
	const FLT x711 = (x120 * (x710 + (-1 * x709 * x175))) + (x709 * x174);
	const FLT x712 = (x709 * x179) + (x711 * x120);
	const FLT x713 = x707 + (-1 * x141 * (x706 + (x712 * x181) + (-1 * x204 * ((x708 * x202) + (-1 * x133 * ((x712 * x120) + (x709 * x188) + (x709 * x182) + (x120 * (x712 + (x709 * x183) + (x120 * (x711 + (x120 * ((-1 * x709 * x186) + x710 + (x709 * x184))) + (x709 * x187))))))))) + (-1 * x708 * x205) + (x709 * x173)));
	const FLT x714 = (x640 * (*error_model).Object.Pose.AxisAngleRot[0]) + (2 * x39) + (-1 * x41 * x554) + (-1 * x639 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x715 = x714 * x559;
	const FLT x716 = x715 * x582;
	const FLT x717 = x714 * x562;
	const FLT x718 = x688 + (-1 * x717 * x557) + x605 + (-1 * x715 * x587) + x651 + (-1 * x644 * x715) + (x716 * x556);
	const FLT x719 = x714 * x560;
	const FLT x720 = x684 + x596 + (-1 * x717 * x582) + x595 + (-1 * x719 * x556) + (-1 * x715 * x567) + (x715 * x574);
	const FLT x721 = (x69 * x719) + x572 + x686 + x646 + (x715 * x598) + (-1 * x717 * x565) + (x716 * x584);
	const FLT x722 = (x718 * sensor_pt[2]) + (-1 * x720 * sensor_pt[1]) + (x721 * sensor_pt[0]);
	const FLT x723 = (-1 * x69 * x716) + x648 + x70 + (x719 * x584) + x690 + (-1 * x603 * x715) + (-1 * x717 * x573);
	const FLT x724 = (x720 * sensor_pt[0]) + (-1 * x723 * sensor_pt[2]) + (x721 * sensor_pt[1]);
	const FLT x725 = (x618 * x724) + (-1 * x616 * x718) + (x723 * x581) + (-1 * x611 * x722);
	const FLT x726 = (x723 * sensor_pt[1]) + (x721 * sensor_pt[2]) + (-1 * x718 * sensor_pt[0]);
	const FLT x727 = (x613 * x722) + (x616 * x720) + (-1 * x614 * x723) + (-1 * x618 * x726);
	const FLT x728 = (-1 * x613 * x724) + (x614 * x718) + (-1 * x720 * x581) + (x611 * x726);
	const FLT x729 = (-1 * x97 * x728) + (x30 * x725) + (x620 * x727);
	const FLT x730 = (-1 * x30 * x727) + (x28 * x728) + (x620 * x725);
	const FLT x731 = x728 + (x729 * x268) + (-1 * x730 * x288);
	const FLT x732 = (-1 * x28 * x725) + (x97 * x727) + (x620 * x728);
	const FLT x733 = x725 + (x732 * x288) + (-1 * x729 * x310);
	const FLT x734 = (x733 * x164) + (x731 * x158);
	const FLT x735 = x727 + (-1 * x732 * x268) + (x730 * x310);
	const FLT x736 = (x735 * x193) + (-1 * x734 * x192);
	const FLT x737 = ((x731 * x196) + (-1 * x733 * x195)) * x197;
	const FLT x738 = (-1 * x737) + (x736 * x191);
	const FLT x739 = (x735 * x119) + (-1 * x167 * (x734 + (x735 * x153)));
	const FLT x740 = x739 * x176;
	const FLT x741 = (x120 * (x740 + (-1 * x739 * x175))) + (x739 * x174);
	const FLT x742 = (x739 * x179) + (x741 * x120);
	const FLT x743 = x737 + (-1 * x141 * (x736 + (-1 * x204 * ((x738 * x202) + (-1 * x133 * ((x739 * x182) + (x742 * x120) + (x120 * (x742 + (x739 * x183) + (x120 * (x741 + (x120 * ((-1 * x739 * x186) + x740 + (x739 * x184))) + (x739 * x187))))) + (x739 * x188))))) + (-1 * x738 * x205) + (x742 * x181) + (x739 * x173)));
	const FLT x744 = -1 * x77;
	const FLT x745 = x44 * x47;
	const FLT x746 = x63 * x40;
	const FLT x747 = 0.5 * x746;
	const FLT x748 = x64 * x747;
	const FLT x749 = dt * dt * dt;
	const FLT x750 = 0.5 * x58 * x749;
	const FLT x751 = x48 * x750;
	const FLT x752 = x47 * x47 * x47;
	const FLT x753 = dt * dt * dt * dt;
	const FLT x754 = 2 * (1. / (x53 * x53)) * x57;
	const FLT x755 = x754 * x753;
	const FLT x756 = 1.0 * x60;
	const FLT x757 = (1. / (x53 * sqrt(x53))) * x56;
	const FLT x758 = x756 * x757;
	const FLT x759 = x758 * x753;
	const FLT x760 = x45 * x47;
	const FLT x761 = x51 * x755;
	const FLT x762 = 2 * x59;
	const FLT x763 = x51 * x759;
	const FLT x764 = x64 * x756;
	const FLT x765 = (x759 * x752) + (x47 * x763) + (-1 * x755 * x752) + (x760 * x759) + (-1 * x760 * x755) + (-1 * x47 * x761) + (-1 * x764 * x745) + (x762 * x745);
	const FLT x766 = (1. / (x61 * sqrt(x61))) * x43;
	const FLT x767 = x766 * x765;
	const FLT x768 = x60 * x766;
	const FLT x769 = x768 * x765;
	const FLT x770 = x63 * x41;
	const FLT x771 = x47 * x770;
	const FLT x772 = x749 * x757;
	const FLT x773 = x33 * x772;
	const FLT x774 = x773 * x771;
	const FLT x775 = x48 * x772;
	const FLT x776 = x33 * x750;
	const FLT x777 = x47 * x776;
	const FLT x778 = x81 * x777;
	const FLT x779 = x766 * x582;
	const FLT x780 = x765 * x556;
	const FLT x781 = x50 * x772;
	const FLT x782 = x63 * x37;
	const FLT x783 = x47 * x782;
	const FLT x784 = x50 * x47 * x750;
	const FLT x785 = (x78 * x784) + (-1 * x781 * x783);
	const FLT x786 = x785 + x778 + (x76 * x775) + (-1 * x748 * x745) + x744 + (x779 * x780) + (-1 * x73 * x751) + (-1 * x767 * x587) + (-1 * x774) + (-1 * x769 * x557) + (-1 * x644 * x767);
	const FLT x787 = x766 * x573;
	const FLT x788 = x766 * x557;
	const FLT x789 = 0.5 * x64;
	const FLT x790 = x789 * x745;
	const FLT x791 = (-1 * x773 * x783) + (x78 * x777);
	const FLT x792 = (-1 * x81 * x784) + (x771 * x781);
	const FLT x793 = (-1 * x775 * x746) + (-1 * x76 * x790) + (x84 * x751) + x792 + x74 + (x69 * x765 * x787) + x791 + (-1 * x769 * x582) + (-1 * x767 * x567) + (-1 * x788 * x780);
	const FLT x794 = x47 * x781 * x746;
	const FLT x795 = x69 * x788;
	const FLT x796 = x765 * x584;
	const FLT x797 = x84 * x784;
	const FLT x798 = -1 * x79;
	const FLT x799 = x47 * x773;
	const FLT x800 = (x76 * x799) + (-1 * x73 * x777);
	const FLT x801 = x800 + (-1 * x769 * x565) + x798 + (-1 * x790 * x782) + (x775 * x770) + x794 + (x767 * x598) + (-1 * x81 * x751) + (x765 * x795) + (x779 * x796) + (-1 * x797);
	const FLT x802 = (x786 * sensor_pt[2]) + (-1 * x793 * sensor_pt[1]) + (x801 * sensor_pt[0]);
	const FLT x803 = x84 * x777;
	const FLT x804 = x73 * x784;
	const FLT x805 = x69 * x779;
	const FLT x806 = x76 * x781;
	const FLT x807 = x47 * x806;
	const FLT x808 = x799 * x746;
	const FLT x809 = (-1 * x775 * x782) + (x78 * x751) + (-1 * x765 * x805) + (x788 * x796) + (-1 * x769 * x573) + x804 + x67 + (-1 * x803) + (-1 * x603 * x767) + (-1 * x770 * x790) + (-1 * x807) + x808;
	const FLT x810 = (x793 * sensor_pt[0]) + (-1 * x809 * sensor_pt[2]) + (x801 * sensor_pt[1]);
	const FLT x811 = (x618 * x810) + (-1 * x616 * x786) + (x581 * x809) + (-1 * x611 * x802);
	const FLT x812 = x21 * x811;
	const FLT x813 = (-1 * x786 * sensor_pt[0]) + (x809 * sensor_pt[1]) + (x801 * sensor_pt[2]);
	const FLT x814 = (-1 * x618 * x813) + (x613 * x802) + (-1 * x614 * x809) + (x616 * x793);
	const FLT x815 = (x614 * x786) + (-1 * x793 * x581) + (-1 * x613 * x810) + (x611 * x813);
	const FLT x816 = (-1 * x97 * x815) + (x11 * x812) + (x620 * x814);
	const FLT x817 = (-1 * x30 * x814) + (x28 * x815) + (x8 * x812);
	const FLT x818 = x815 + (x816 * x268) + (-1 * x817 * x288);
	const FLT x819 = (-1 * x4 * x812) + (x97 * x814) + (x620 * x815);
	const FLT x820 = (x819 * x288) + x811 + (-1 * x816 * x310);
	const FLT x821 = (x820 * x164) + (x818 * x158);
	const FLT x822 = x814 + (x817 * x310) + (-1 * x819 * x268);
	const FLT x823 = (x822 * x193) + (-1 * x821 * x192);
	const FLT x824 = ((x818 * x196) + (-1 * x820 * x195)) * x197;
	const FLT x825 = (-1 * x824) + (x823 * x191);
	const FLT x826 = (x822 * x119) + (-1 * x167 * (x821 + (x822 * x153)));
	const FLT x827 = x826 * x176;
	const FLT x828 = (x120 * (x827 + (-1 * x826 * x175))) + (x826 * x174);
	const FLT x829 = (x826 * x179) + (x828 * x120);
	const FLT x830 = x824 + (-1 * x141 * (x823 + (x829 * x181) + (-1 * x204 * ((x825 * x202) + (-1 * x133 * ((x829 * x120) + (x826 * x188) + (x826 * x182) + (x120 * (x829 + (x826 * x183) + (x120 * (x828 + (x120 * ((-1 * x826 * x186) + x827 + (x826 * x184))) + (x826 * x187))))))))) + (-1 * x825 * x205) + (x826 * x173)));
	const FLT x831 = x51 * x772;
	const FLT x832 = x33 * x781;
	const FLT x833 = x770 * x832;
	const FLT x834 = x51 * x750;
	const FLT x835 = x50 * x755;
	const FLT x836 = x50 * x759;
	const FLT x837 = x68 * x44;
	const FLT x838 = (x50 * x50 * x50) * x753;
	const FLT x839 = x44 * x762;
	const FLT x840 = (x50 * x839) + (x758 * x838) + (-1 * x754 * x838) + (x48 * x836) + (-1 * x756 * x837) + (x45 * x836) + (-1 * x48 * x835) + (-1 * x45 * x835);
	const FLT x841 = x766 * x840;
	const FLT x842 = x69 * x841;
	const FLT x843 = x768 * x840;
	const FLT x844 = x584 * x840;
	const FLT x845 = x50 * x776;
	const FLT x846 = x81 * x845;
	const FLT x847 = x556 * x840;
	const FLT x848 = (x78 * x834) + (x779 * x847) + (-1 * x833) + (-1 * x557 * x843) + x67 + (-1 * x804) + (-1 * x782 * x831) + x807 + (-1 * x565 * x842) + (-1 * x787 * x844) + (-1 * x747 * x837) + x846;
	const FLT x849 = 0.5 * x837;
	const FLT x850 = (-1 * x84 * x845) + (x746 * x832);
	const FLT x851 = x785 + (-1 * x770 * x849) + x850 + (-1 * x805 * x840) + x77 + (x73 * x834) + (-1 * x76 * x831) + (-1 * x573 * x843) + (-1 * x603 * x841) + (x788 * x844);
	const FLT x852 = -1 * x74;
	const FLT x853 = x63 * x831;
	const FLT x854 = x33 * x806;
	const FLT x855 = x73 * x845;
	const FLT x856 = x792 + (-1 * x782 * x849) + (x795 * x840) + (-1 * x855) + (x779 * x844) + (x598 * x841) + x852 + (x40 * x853) + (-1 * x84 * x834) + x854 + (-1 * x565 * x843);
	const FLT x857 = (x78 * x845) + (-1 * x782 * x832);
	const FLT x858 = (-1 * x794) + (-1 * x582 * x843) + (x573 * x842) + (-1 * x567 * x841) + x857 + (-1 * x81 * x834) + (x41 * x853) + x797 + x798 + (-1 * x76 * x849) + (-1 * x788 * x847);
	const FLT x859 = (-1 * x851 * sensor_pt[2]) + (x858 * sensor_pt[0]) + (x856 * sensor_pt[1]);
	const FLT x860 = (-1 * x858 * sensor_pt[1]) + (x848 * sensor_pt[2]) + (x856 * sensor_pt[0]);
	const FLT x861 = (x618 * x859) + (-1 * x611 * x860) + (-1 * x616 * x848) + (x581 * x851);
	const FLT x862 = (-1 * x848 * sensor_pt[0]) + (x851 * sensor_pt[1]) + (x856 * sensor_pt[2]);
	const FLT x863 = (x613 * x860) + (-1 * x618 * x862) + (-1 * x614 * x851) + (x616 * x858);
	const FLT x864 = (x614 * x848) + (-1 * x581 * x858) + (-1 * x613 * x859) + (x611 * x862);
	const FLT x865 = (-1 * x97 * x864) + (x30 * x861) + (x620 * x863);
	const FLT x866 = (x28 * x864) + (-1 * x30 * x863) + (x620 * x861);
	const FLT x867 = x864 + (x865 * x268) + (-1 * x866 * x288);
	const FLT x868 = (-1 * x28 * x861) + (x620 * x864) + (x97 * x863);
	const FLT x869 = x861 + (x868 * x288) + (-1 * x865 * x310);
	const FLT x870 = (x869 * x164) + (x867 * x158);
	const FLT x871 = (-1 * x868 * x268) + x863 + (x866 * x310);
	const FLT x872 = (x871 * x193) + (-1 * x870 * x192);
	const FLT x873 = ((x867 * x196) + (-1 * x869 * x195)) * x197;
	const FLT x874 = (-1 * x873) + (x872 * x191);
	const FLT x875 = (x871 * x119) + (-1 * x167 * (x870 + (x871 * x153)));
	const FLT x876 = x875 * x176;
	const FLT x877 = (x120 * (x876 + (-1 * x875 * x175))) + (x875 * x174);
	const FLT x878 = (x875 * x179) + (x877 * x120);
	const FLT x879 = x873 + (-1 * x141 * (x872 + (-1 * x204 * ((x874 * x202) + (-1 * x133 * ((x878 * x120) + (x875 * x188) + (x875 * x182) + (x120 * (x878 + (x875 * x183) + (x120 * ((x120 * ((-1 * x875 * x186) + x876 + (x875 * x184))) + x877 + (x875 * x187))))))))) + (x878 * x181) + (-1 * x874 * x205) + (x875 * x173)));
	const FLT x880 = x48 * x33;
	const FLT x881 = x44 * x33;
	const FLT x882 = x33 * x33 * x33;
	const FLT x883 = (x759 * x882) + (-1 * x755 * x882) + (-1 * x755 * x880) + (x759 * x880) + (x33 * x763) + (-1 * x33 * x761) + (-1 * x764 * x881) + (x33 * x839);
	const FLT x884 = x766 * x883;
	const FLT x885 = x45 * x772;
	const FLT x886 = x63 * x885;
	const FLT x887 = x69 * x884;
	const FLT x888 = x45 * x750;
	const FLT x889 = x789 * x881;
	const FLT x890 = x768 * x883;
	const FLT x891 = (-1 * x37 * x886) + (x573 * x887) + (-1 * x567 * x884) + x67 + x803 + x833 + (x78 * x888) + (-1 * x808) + (-1 * x76 * x889) + (-1 * x557 * x556 * x884) + (-1 * x846) + (-1 * x582 * x890);
	const FLT x892 = x584 * x884;
	const FLT x893 = x582 * x884;
	const FLT x894 = x791 + x855 + (x557 * x892) + (-1 * x573 * x890) + (-1 * x603 * x884) + (-1 * x854) + (x40 * x886) + (-1 * x69 * x893) + x852 + (-1 * x770 * x889) + (-1 * x84 * x888);
	const FLT x895 = x850 + (-1 * x565 * x890) + (x598 * x884) + (x557 * x887) + (-1 * x73 * x888) + (-1 * x778) + (x584 * x893) + (-1 * x782 * x889) + (x76 * x885) + x774 + x744;
	const FLT x896 = (-1 * x894 * sensor_pt[2]) + (x891 * sensor_pt[0]) + (x895 * sensor_pt[1]);
	const FLT x897 = x857 + x800 + (-1 * x748 * x881) + x79 + (-1 * x41 * x886) + (-1 * x557 * x890) + (-1 * x573 * x892) + (-1 * x565 * x887) + (x556 * x893) + (x81 * x888);
	const FLT x898 = (x895 * sensor_pt[2]) + (x894 * sensor_pt[1]) + (-1 * x897 * sensor_pt[0]);
	const FLT x899 = (x611 * x898) + (-1 * x581 * x891) + (x614 * x897) + (-1 * x613 * x896);
	const FLT x900 = (x897 * sensor_pt[2]) + (-1 * x891 * sensor_pt[1]) + (x895 * sensor_pt[0]);
	const FLT x901 = (-1 * x900 * x611) + (x618 * x896) + (-1 * x616 * x897) + (x581 * x894);
	const FLT x902 = (x900 * x613) + (-1 * x614 * x894) + (-1 * x618 * x898) + (x616 * x891);
	const FLT x903 = (x28 * x899) + (-1 * x30 * x902) + (x901 * x620);
	const FLT x904 = (-1 * x28 * x901) + (x97 * x902) + (x620 * x899);
	const FLT x905 = x902 + (x903 * x310) + (-1 * x904 * x268);
	const FLT x906 = x21 * ((-1 * x97 * x899) + (x30 * x901) + (x902 * x620));
	const FLT x907 = x899 + (x906 * x143) + (-1 * x903 * x288);
	const FLT x908 = x901 + (x904 * x288) + (-1 * x906 * x309);
	const FLT x909 = (x908 * x164) + (x907 * x158);
	const FLT x910 = x170 * ((x905 * x119) + (-1 * x167 * (x909 + (x905 * x153))));
	const FLT x911 = x126 * x172 * x136;
	const FLT x912 = x910 * x122;
	const FLT x913 = (x120 * (x912 + (-1 * x910 * x121))) + (x910 * x123);
	const FLT x914 = (x910 * x124) + (x913 * x120);
	const FLT x915 = (x905 * x193) + (-1 * x909 * x192);
	const FLT x916 = ((x907 * x196) + (-1 * x908 * x195)) * x197;
	const FLT x917 = (-1 * x916) + (x915 * x191);
	const FLT x918 = x916 + (-1 * x141 * ((-1 * x917 * x205) + (x911 * x910) + x915 + (-1 * x204 * ((x917 * x202) + (-1 * x133 * ((x910 * x125) + (x914 * x120) + (x910 * x130) + (x120 * (x914 + (x910 * x129) + (x120 * (x913 + (x120 * ((-1 * x910 * x185) + x912 + (x910 * x127))) + (x910 * x128))))))))) + (x914 * x181)));
	const FLT x919 = -1 * dt * x155;
	const FLT x920 = (-1 * dt * x156) + dt;
	const FLT x921 = x920 + x919;
	const FLT x922 = dt * x161;
	const FLT x923 = dt * x159;
	const FLT x924 = x923 + x922;
	const FLT x925 = (x924 * x164) + (x921 * x158);
	const FLT x926 = dt * x150;
	const FLT x927 = dt * x146;
	const FLT x928 = (-1 * x927) + x926;
	const FLT x929 = (x928 * x193) + (-1 * x925 * x192);
	const FLT x930 = ((x921 * x196) + (-1 * x924 * x195)) * x197;
	const FLT x931 = (-1 * x930) + (x929 * x191);
	const FLT x932 = (x928 * x119) + (-1 * x167 * (x925 + (x928 * x153)));
	const FLT x933 = x932 * x176;
	const FLT x934 = (x120 * (x933 + (-1 * x932 * x175))) + (x932 * x174);
	const FLT x935 = (x932 * x179) + (x934 * x120);
	const FLT x936 = x930 + (-1 * x141 * ((-1 * x204 * ((x931 * x202) + (-1 * x133 * ((x932 * x188) + (x120 * (x935 + (x932 * x183) + (x120 * (x934 + (x120 * (x933 + (-1 * x932 * x186) + (x932 * x184))) + (x932 * x187))))) + (x935 * x120) + (x932 * x182))))) + x929 + (x935 * x181) + (-1 * x931 * x205) + (x932 * x173)));
	const FLT x937 = -1 * dt * x218;
	const FLT x938 = x920 + x937;
	const FLT x939 = x926 + x927;
	const FLT x940 = dt * x214;
	const FLT x941 = dt * x213;
	const FLT x942 = (-1 * x941) + x940;
	const FLT x943 = (x942 * x164) + (x939 * x158);
	const FLT x944 = (x938 * x119) + (-1 * x167 * (x943 + (x938 * x153)));
	const FLT x945 = x944 * x170;
	const FLT x946 = x944 * x176;
	const FLT x947 = (x120 * (x946 + (-1 * x945 * x121))) + (x944 * x174);
	const FLT x948 = (x944 * x179) + (x947 * x120);
	const FLT x949 = (x938 * x193) + (-1 * x943 * x192);
	const FLT x950 = ((x939 * x196) + (-1 * x942 * x195)) * x197;
	const FLT x951 = x200 * ((-1 * x950) + (x949 * x191));
	const FLT x952 = x950 + (-1 * x141 * (x949 + (-1 * x204 * ((x951 * x201) + (-1 * x133 * ((x944 * x188) + (x948 * x120) + (x944 * x182) + (x120 * (x948 + (x944 * x183) + (x120 * (x947 + (x120 * ((-1 * x945 * x185) + x946 + (x944 * x184))) + (x944 * x187))))))))) + (-1 * x951 * x139) + (x945 * x911) + (x948 * x181)));
	const FLT x953 = (-1 * x922) + x923;
	const FLT x954 = x937 + x919 + dt;
	const FLT x955 = (x954 * x164) + (x953 * x158);
	const FLT x956 = x940 + x941;
	const FLT x957 = (x956 * x193) + (-1 * x955 * x192);
	const FLT x958 = ((x953 * x196) + (-1 * x954 * x195)) * x197;
	const FLT x959 = (-1 * x958) + (x957 * x191);
	const FLT x960 = (x956 * x119) + (-1 * x167 * (x955 + (x956 * x153)));
	const FLT x961 = x960 * x170;
	const FLT x962 = x961 * x122;
	const FLT x963 = (x120 * (x962 + (-1 * x961 * x121))) + (x961 * x123);
	const FLT x964 = (x961 * x124) + (x963 * x120);
	const FLT x965 = x958 + (-1 * x141 * ((x961 * x911) + x957 + (-1 * x204 * ((x959 * x202) + (-1 * x133 * ((x964 * x120) + (x961 * x125) + (x960 * x182) + (x120 * (x964 + (x961 * x129) + (x120 * (x963 + (x120 * ((-1 * x961 * x185) + x962 + (x961 * x127))) + (x961 * x128))))))))) + (-1 * x959 * x205) + (x964 * x181)));
	const FLT x966 = x203 * x134;
	const FLT x967 = (x139 + x966) * x141;
	const FLT x968 = ((-1 * x112 * x139) + (-1 * x966 * x112)) * x141;
	const FLT x969 = x141 * (x205 + (x966 * x200));
	const FLT x970 = x109 * (1 + x190);
	const FLT x971 = x116 * x169;
	const FLT x972 = x971 * x105 * x132;
	const FLT x973 = x972 * x176;
	const FLT x974 = (x120 * (x973 + (-1 * x972 * x175))) + (x972 * x174);
	const FLT x975 = (x972 * x179) + (x974 * x120);
	const FLT x976 = x970 * x191;
	const FLT x977 = x141 * ((x971 * x171 * x133 * x153) + (-1 * x976 * x205) + (-1 * x204 * ((-1 * x133 * ((x975 * x120) + (x972 * x182) + (x972 * x188) + (x120 * (x975 + (x972 * x183) + (x120 * (x974 + (x120 * (x973 + (-1 * x972 * x186) + (x972 * x184))) + (x972 * x187))))))) + (x976 * x202) + (-1 * x132) + (-1 * x113 * x117 * x131))) + x970 + (x975 * x181));
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), x206 + (x208 * x206));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), x228 + (x208 * x228));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), x241 + (x208 * x241));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x343 + (x208 * x343));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x399 + (x208 * x399));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x433 + (x433 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x463 + (x463 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), x485 + (x485 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), x500 + (x500 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), x512 + (x512 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), x526 + (x526 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), x539 + (x539 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), x551 + (x551 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x637 + (x637 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x679 + (x679 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x713 + (x713 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x743 + (x743 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x830 + (x830 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x879 + (x879 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x918 + (x918 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), x936 + (x936 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), x952 + (x952 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), x965 + (x965 * x208));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.curve)/sizeof(FLT), (-1 * x967 * x208) + (-1 * x967));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.gibmag)/sizeof(FLT), sin(x207));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.gibpha)/sizeof(FLT), x208);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.ogeemag)/sizeof(FLT), (-1 * x968 * x208) + (-1 * x968));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.ogeephase)/sizeof(FLT), (-1 * x969 * x208) + (-1 * x969));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.phase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD0.tilt)/sizeof(FLT), (-1 * x977 * x208) + (-1 * x977));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]

static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_x0(Hx, dt, _x0, error_model, sensor_pt);
    }
}
// Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_error_model(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x1 = 0.5 * (*_x0).Lighthouse.Rot[2];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x3 = (*_x0).Lighthouse.Rot[0] + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (-1 * x0 * (*_x0).Lighthouse.Rot[3]) + (-1 * x1 * (*error_model).Lighthouse.AxisAngleRot[1]);
	const FLT x4 = x3 * x3;
	const FLT x5 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x6 = (x5 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x0 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x7 = x6 * x6;
	const FLT x8 = (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (x2 * (*_x0).Lighthouse.Rot[3]) + (x5 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2];
	const FLT x9 = x8 * x8;
	const FLT x10 = (-1 * x5 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[1] + (x2 * (*_x0).Lighthouse.Rot[0]) + (x1 * (*error_model).Lighthouse.AxisAngleRot[2]);
	const FLT x11 = x10 * x10;
	const FLT x12 = x11 + x9 + x7 + x4;
	const FLT x13 = 1. / (x12 * x12);
	const FLT x14 = 1.0 * x10;
	const FLT x15 = x14 * (*_x0).Lighthouse.Rot[0];
	const FLT x16 = 1.0 * (*_x0).Lighthouse.Rot[3];
	const FLT x17 = x8 * x16;
	const FLT x18 = 1.0 * x6;
	const FLT x19 = x18 * (*_x0).Lighthouse.Rot[2];
	const FLT x20 = 1.0 * x3;
	const FLT x21 = x20 * (*_x0).Lighthouse.Rot[1];
	const FLT x22 = (-1 * x21) + (-1 * x19) + x15 + x17;
	const FLT x23 = x22 * x13;
	const FLT x24 = 1. / x12;
	const FLT x25 = (-1 * x24 * x21) + (-1 * x9 * x23) + (-1 * x24 * x19) + (x24 * x15) + (-1 * x4 * x23) + (-1 * x7 * x23) + (x24 * x17) + (-1 * x23 * x11);
	const FLT x26 = 1. / sqrt(x12);
	const FLT x27 = x7 * x24;
	const FLT x28 = x9 * x24;
	const FLT x29 = x24 * x11;
	const FLT x30 = x29 + x27 + (x4 * x24) + x28;
	const FLT x31 = (1. / (x30 * sqrt(x30))) * x26;
	const FLT x32 = x6 * x31;
	const FLT x33 = x32 * x25;
	const FLT x34 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x35 = 1.0/2.0 * x34;
	const FLT x36 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x37 = 1.0/2.0 * x36;
	const FLT x38 = 1. / sqrt(x30);
	const FLT x39 = x38 * (1. / (x12 * sqrt(x12)));
	const FLT x40 = x39 * x22;
	const FLT x41 = x3 * x40;
	const FLT x42 = x8 * x31;
	const FLT x43 = x42 * x25;
	const FLT x44 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x45 = 1.0/2.0 * x44;
	const FLT x46 = x6 * x40;
	const FLT x47 = x8 * x40;
	const FLT x48 = x31 * x25;
	const FLT x49 = x3 * x48;
	const FLT x50 = x38 * x26;
	const FLT x51 = x50 * (*_x0).Lighthouse.Rot[1];
	const FLT x52 = 0.5 * x51;
	const FLT x53 = x1 * x50;
	const FLT x54 = 0.5 * (*_x0).Lighthouse.Rot[3];
	const FLT x55 = x50 * x54;
	const FLT x56 = (-1 * x55 * x44) + (-1 * x52 * x36) + (-1 * x53 * x34);
	const FLT x57 = x56 + (-1 * x49 * x37) + (x45 * x47) + (-1 * x33 * x35) + (-1 * x41 * x37) + (x43 * x45) + (-1 * x46 * x35);
	const FLT x58 = x6 * x50;
	const FLT x59 = 2 * x58;
	const FLT x60 = x55 * x36;
	const FLT x61 = x40 * x10;
	const FLT x62 = x52 * x44;
	const FLT x63 = x50 * (*_x0).Lighthouse.Rot[0];
	const FLT x64 = 0.5 * x63;
	const FLT x65 = x64 * x34;
	const FLT x66 = x48 * x10;
	const FLT x67 = (x66 * x35) + (-1 * x47 * x37) + (-1 * x65) + (-1 * x62) + x60 + (-1 * x43 * x37) + (x61 * x35) + (-1 * x41 * x45) + (-1 * x45 * x49);
	const FLT x68 = x50 * x10;
	const FLT x69 = 2 * x68;
	const FLT x70 = x8 * x50;
	const FLT x71 = x3 * x50;
	const FLT x72 = (x58 * x34) + (-1 * x70 * x44) + (x71 * x36);
	const FLT x73 = dt * fabs(dt);
	const FLT x74 = 1.0/2.0 * x73;
	const FLT x75 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x76 = dt * dt;
	const FLT x77 = x75 * x75;
	const FLT x78 = x77 * x76;
	const FLT x79 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x80 = x79 * x79;
	const FLT x81 = x80 * x76;
	const FLT x82 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x83 = x82 * x82;
	const FLT x84 = x83 * x76;
	const FLT x85 = 1e-10 + x84 + x78 + x81;
	const FLT x86 = sqrt(x85);
	const FLT x87 = 0.5 * x86;
	const FLT x88 = sin(x87);
	const FLT x89 = x88 * (1. / x86);
	const FLT x90 = x89 * x75;
	const FLT x91 = x88 * x88;
	const FLT x92 = 1. / x85;
	const FLT x93 = x92 * x91;
	const FLT x94 = cos(x87);
	const FLT x95 = (x81 * x93) + (x78 * x93) + (x94 * x94) + (x84 * x93);
	const FLT x96 = 1. / sqrt(x95);
	const FLT x97 = dt * x96;
	const FLT x98 = x90 * x97;
	const FLT x99 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x100 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x101 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x102 = (-1 * x101 * (*_x0).Object.Pose.Rot[1]) + (-1 * x99 * (*_x0).Object.Pose.Rot[3]) + (*_x0).Object.Pose.Rot[0] + (-1 * x100 * (*_x0).Object.Pose.Rot[2]);
	const FLT x103 = (x100 * (*_x0).Object.Pose.Rot[1]) + (*_x0).Object.Pose.Rot[3] + (-1 * x101 * (*_x0).Object.Pose.Rot[2]) + (x99 * (*_x0).Object.Pose.Rot[0]);
	const FLT x104 = (-1 * x99 * (*_x0).Object.Pose.Rot[1]) + (x101 * (*_x0).Object.Pose.Rot[3]) + (x100 * (*_x0).Object.Pose.Rot[0]) + (*_x0).Object.Pose.Rot[2];
	const FLT x105 = (*_x0).Object.Pose.Rot[1] + (-1 * x100 * (*_x0).Object.Pose.Rot[3]) + (x101 * (*_x0).Object.Pose.Rot[0]) + (x99 * (*_x0).Object.Pose.Rot[2]);
	const FLT x106 = (x105 * x105) + (x102 * x102) + (x103 * x103) + (x104 * x104);
	const FLT x107 = 1. / sqrt(x106);
	const FLT x108 = x102 * x107;
	const FLT x109 = x89 * x97;
	const FLT x110 = x109 * x107;
	const FLT x111 = x105 * x110;
	const FLT x112 = x96 * x94;
	const FLT x113 = x103 * x107;
	const FLT x114 = x112 * x113;
	const FLT x115 = x104 * x107;
	const FLT x116 = x109 * x115;
	const FLT x117 = (x79 * x116) + x114 + (x98 * x108) + (-1 * x82 * x111);
	const FLT x118 = x107 * x112;
	const FLT x119 = x102 * x118;
	const FLT x120 = (-1 * x98 * x113) + (-1 * x79 * x111) + x119 + (-1 * x82 * x116);
	const FLT x121 = x109 * x113;
	const FLT x122 = x105 * x118;
	const FLT x123 = x102 * x110;
	const FLT x124 = (x79 * x123) + x122 + (-1 * x98 * x115) + (x82 * x121);
	const FLT x125 = (-1 * x124 * sensor_pt[2]) + (x120 * sensor_pt[1]) + (x117 * sensor_pt[0]);
	const FLT x126 = x104 * x118;
	const FLT x127 = x105 * x107;
	const FLT x128 = (x98 * x127) + (x82 * x123) + (-1 * x79 * x121) + x126;
	const FLT x129 = (x120 * sensor_pt[2]) + (-1 * x128 * sensor_pt[0]) + (x124 * sensor_pt[1]);
	const FLT x130 = (*_x0).Object.Pose.Pos[0] + (x74 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (2 * ((x128 * x129) + (-1 * x117 * x125))) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0];
	const FLT x131 = 1.0/2.0 * x130;
	const FLT x132 = x50 * x130;
	const FLT x133 = x54 * x132;
	const FLT x134 = (-1 * x117 * sensor_pt[1]) + (x120 * sensor_pt[0]) + (x128 * sensor_pt[2]);
	const FLT x135 = (*_x0).Object.Pose.Pos[1] + (x74 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x117 * x134) + (-1 * x124 * x129)));
	const FLT x136 = 1.0/2.0 * x135;
	const FLT x137 = (*_x0).Object.Pose.Pos[2] + (2 * ((x124 * x125) + (-1 * x128 * x134))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x74 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x138 = x52 * x137;
	const FLT x139 = 1.0/2.0 * x137;
	const FLT x140 = x50 * x135;
	const FLT x141 = 0.5 * x140;
	const FLT x142 = x141 * (*_x0).Lighthouse.Rot[0];
	const FLT x143 = (-1 * x43 * x131) + (-1 * x142) + (x66 * x136) + (-1 * x49 * x139) + x133 + (-1 * x138) + (-1 * x47 * x131) + (x61 * x136) + (-1 * x41 * x139);
	const FLT x144 = (x70 * x130) + (x71 * x137) + (-1 * x10 * x140);
	const FLT x145 = 1.0 * x144;
	const FLT x146 = x63 * x145;
	const FLT x147 = (x58 * x135) + (x71 * x130) + (-1 * x70 * x137);
	const FLT x148 = x25 * x147;
	const FLT x149 = 1.0 * x147;
	const FLT x150 = x50 * (*_x0).Lighthouse.Rot[2];
	const FLT x151 = 1.0 * x150;
	const FLT x152 = x10 * x144;
	const FLT x153 = (x70 * x36) + (x71 * x44) + (-1 * x68 * x34);
	const FLT x154 = x10 * x153;
	const FLT x155 = 1.0 * x153;
	const FLT x156 = x63 * x155;
	const FLT x157 = x50 * x137;
	const FLT x158 = (-1 * x54 * x157) + (-1 * x1 * x140) + (-1 * x52 * x130);
	const FLT x159 = x158 + (x43 * x139) + (-1 * x41 * x131) + (-1 * x33 * x136) + (x47 * x139) + (-1 * x46 * x136) + (-1 * x49 * x131);
	const FLT x160 = (-1 * x59 * x159) + (x40 * x154) + (-1 * x48 * x152) + (-1 * x156) + (x69 * x143) + (-1 * x72 * x33) + x146 + (x57 * x59) + (-1 * x67 * x69) + (x32 * x148) + (x149 * x150) + (x46 * x147) + (-1 * x40 * x152) + (-1 * x72 * x151) + (-1 * x72 * x46) + (x48 * x154);
	const FLT x161 = x135 + (2 * ((-1 * x58 * x147) + (x68 * x144))) + (-1 * (x34 + (2 * ((-1 * x72 * x58) + (x68 * x153)))));
	const FLT x162 = 2 * x161;
	const FLT x163 = 2 * x70;
	const FLT x164 = (x68 * x137) + (x3 * x140) + (-1 * x58 * x130);
	const FLT x165 = x6 * x164;
	const FLT x166 = (x71 * x34) + (x68 * x44) + (-1 * x58 * x36);
	const FLT x167 = x40 * x166;
	const FLT x168 = x64 * x137;
	const FLT x169 = x141 * (*_x0).Lighthouse.Rot[1];
	const FLT x170 = x1 * x132;
	const FLT x171 = (x33 * x131) + x170 + (-1 * x49 * x136) + (x46 * x131) + (-1 * x66 * x139) + (-1 * x61 * x139) + x168 + (-1 * x169) + (-1 * x41 * x136);
	const FLT x172 = x64 * x44;
	const FLT x173 = x52 * x34;
	const FLT x174 = x53 * x36;
	const FLT x175 = 2 * (x174 + (x33 * x37) + (-1 * x173) + (-1 * x61 * x45) + (x46 * x37) + (-1 * x49 * x35) + (-1 * x41 * x35) + x172 + (-1 * x66 * x45));
	const FLT x176 = 1.0 * x164;
	const FLT x177 = (x166 * x151) + (-1 * x176 * x150);
	const FLT x178 = x50 * x16;
	const FLT x179 = (x178 * x153) + (-1 * x178 * x144);
	const FLT x180 = x179 + (-1 * x48 * x165) + (x6 * x167) + (-1 * x40 * x165) + (x43 * x144) + (-1 * x47 * x153) + (-1 * x163 * x143) + (-1 * x58 * x175) + (-1 * x43 * x153) + x177 + (x47 * x144) + (x59 * x171) + (x67 * x163) + (x33 * x166);
	const FLT x181 = (-1 * (x36 + (2 * ((-1 * x70 * x153) + (x58 * x166))))) + x130 + (2 * ((-1 * x70 * x144) + (x58 * x164)));
	const FLT x182 = 2 * x181;
	const FLT x183 = x10 * x164;
	const FLT x184 = x10 * x166;
	const FLT x185 = x63 * x176;
	const FLT x186 = 1.0 * x166;
	const FLT x187 = x63 * x186;
	const FLT x188 = (-1 * x72 * x178) + (x68 * x175) + (-1 * x69 * x171) + x187 + (-1 * x10 * x167) + (x48 * x183) + (-1 * x57 * x163) + (-1 * x42 * x148) + (-1 * x48 * x184) + (x40 * x183) + (x72 * x43) + (-1 * x47 * x147) + (x178 * x147) + (x163 * x159) + (x72 * x47) + (-1 * x185);
	const FLT x189 = x137 + (-1 * (x44 + (2 * ((-1 * x68 * x166) + (x70 * x72))))) + (2 * ((-1 * x68 * x164) + (x70 * x147)));
	const FLT x190 = 2 * x189;
	const FLT x191 = (x188 * x190) + (x180 * x182);
	const FLT x192 = 0.523598775598299 + (*error_model).BSD0.tilt + (*_x0).BSD0.tilt;
	const FLT x193 = cos(x192);
	const FLT x194 = 1. / x193;
	const FLT x195 = x161 * x161;
	const FLT x196 = x181 * x181;
	const FLT x197 = x196 + (x189 * x189);
	const FLT x198 = x197 + x195;
	const FLT x199 = 1.0/2.0 * x161;
	const FLT x200 = (1. / (x198 * sqrt(x198))) * x199 * x194;
	const FLT x201 = 1. / sqrt(x198);
	const FLT x202 = x201 * x194;
	const FLT x203 = (x202 * x160) + (-1 * x200 * (x191 + (x160 * x162)));
	const FLT x204 = asin(x202 * x161);
	const FLT x205 = 8.0108022e-06 * x204;
	const FLT x206 = -8.0108022e-06 + (-1 * x205);
	const FLT x207 = 0.0028679863 + (x204 * x206);
	const FLT x208 = 1. / (x193 * x193);
	const FLT x209 = 1. / sqrt(1 + (-1 * x208 * (1. / x198) * x195));
	const FLT x210 = x209 * x207;
	const FLT x211 = x209 * x205;
	const FLT x212 = x209 * x206;
	const FLT x213 = x212 * x203;
	const FLT x214 = (x204 * (x213 + (-1 * x211 * x203))) + (x210 * x203);
	const FLT x215 = 5.3685255e-06 + (x204 * x207);
	const FLT x216 = x215 * x209;
	const FLT x217 = (x216 * x203) + (x214 * x204);
	const FLT x218 = (*error_model).BSD0.ogeemag + (*_x0).BSD0.ogeemag;
	const FLT x219 = atan2(-1 * x189, x181);
	const FLT x220 = 1. / sqrt(x197);
	const FLT x221 = tan(x192);
	const FLT x222 = x220 * x221;
	const FLT x223 = x222 * x161;
	const FLT x224 = asin(x223) + (-1 * x219) + (-1 * (*error_model).BSD0.ogeephase) + (-1 * (*_x0).BSD0.ogeephase);
	const FLT x225 = sin(x224);
	const FLT x226 = (*_x0).BSD0.curve + (-1 * x218 * x225) + (*error_model).BSD0.curve;
	const FLT x227 = 0.0076069798 + (x215 * x204);
	const FLT x228 = x204 * x227;
	const FLT x229 = -8.0108022e-06 + (-1.60216044e-05 * x204);
	const FLT x230 = x207 + (x204 * x229);
	const FLT x231 = x215 + (x230 * x204);
	const FLT x232 = x227 + (x231 * x204);
	const FLT x233 = (x232 * x204) + x228;
	const FLT x234 = sin(x192);
	const FLT x235 = x234 * x226;
	const FLT x236 = x233 * x235;
	const FLT x237 = x193 + (-1 * x236);
	const FLT x238 = 1. / x237;
	const FLT x239 = x204 * x204;
	const FLT x240 = x238 * x239;
	const FLT x241 = x226 * x240;
	const FLT x242 = x238 * x209 * x228;
	const FLT x243 = 2 * x226;
	const FLT x244 = x242 * x243;
	const FLT x245 = 1. / x197;
	const FLT x246 = x221 * x221;
	const FLT x247 = 1. / sqrt(1 + (-1 * x245 * x246 * x195));
	const FLT x248 = x221 * (1. / (x197 * sqrt(x197))) * x199;
	const FLT x249 = (x222 * x160) + (-1 * x248 * x191);
	const FLT x250 = 1. / x181;
	const FLT x251 = x189 * (1. / x196);
	const FLT x252 = x245 * x196;
	const FLT x253 = ((x251 * x180) + (-1 * x250 * x188)) * x252;
	const FLT x254 = (-1 * x253) + (x247 * x249);
	const FLT x255 = x227 * x240;
	const FLT x256 = x218 * cos(x224);
	const FLT x257 = x256 * x255;
	const FLT x258 = x232 * x209;
	const FLT x259 = x231 * x209;
	const FLT x260 = x209 * x229;
	const FLT x261 = 2.40324066e-05 * x204;
	const FLT x262 = x209 * x261;
	const FLT x263 = x230 * x209;
	const FLT x264 = x209 * x227;
	const FLT x265 = x234 * x233;
	const FLT x266 = x265 * x256;
	const FLT x267 = (1. / (x237 * x237)) * x239 * x227;
	const FLT x268 = x267 * x226;
	const FLT x269 = x223 + (x255 * x226);
	const FLT x270 = 1. / sqrt(1 + (-1 * (x269 * x269)));
	const FLT x271 = x253 + (-1 * x270 * ((-1 * x268 * ((x266 * x254) + (-1 * x235 * ((x217 * x204) + (x203 * x258) + (x203 * x264) + (x204 * (x217 + (x203 * x259) + (x204 * (x214 + (x204 * ((-1 * x203 * x262) + x213 + (x203 * x260))) + (x203 * x263))))))))) + x249 + (-1 * x254 * x257) + (x217 * x241) + (x203 * x244)));
	const FLT x272 = (-1 * asin(x269)) + x219 + (*error_model).BSD0.gibpha + (*_x0).BSD0.gibpha;
	const FLT x273 = cos(x272) * ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag);
	const FLT x274 = x10 * x16;
	const FLT x275 = 1.0 * x8;
	const FLT x276 = x275 * (*_x0).Lighthouse.Rot[0];
	const FLT x277 = x18 * (*_x0).Lighthouse.Rot[1];
	const FLT x278 = x20 * (*_x0).Lighthouse.Rot[2];
	const FLT x279 = (-1 * x278) + x277 + (-1 * x274) + x276;
	const FLT x280 = x39 * x279;
	const FLT x281 = x6 * x280;
	const FLT x282 = x13 * x279;
	const FLT x283 = x31 * ((-1 * x9 * x282) + (x24 * x276) + (-1 * x11 * x282) + (-1 * x24 * x278) + (x24 * x277) + (-1 * x24 * x274) + (-1 * x7 * x282) + (-1 * x4 * x282));
	const FLT x284 = 1.0/2.0 * x283;
	const FLT x285 = x3 * x284;
	const FLT x286 = x3 * x280;
	const FLT x287 = x10 * x280;
	const FLT x288 = x10 * x284;
	const FLT x289 = x6 * x284;
	const FLT x290 = (-1 * x285 * x135) + (x281 * x131) + (-1 * x286 * x136) + x158 + (-1 * x287 * x139) + (x289 * x130) + (-1 * x288 * x137);
	const FLT x291 = x8 * x153;
	const FLT x292 = x8 * x280;
	const FLT x293 = x8 * x283;
	const FLT x294 = x53 * x44;
	const FLT x295 = x55 * x34;
	const FLT x296 = x64 * x36;
	const FLT x297 = x296 + (-1 * x45 * x286) + (-1 * x294) + (x34 * x288) + (-1 * x37 * x293) + (-1 * x37 * x292) + (x35 * x287) + x295 + (-1 * x44 * x285);
	const FLT x298 = x64 * x130;
	const FLT x299 = x1 * x157;
	const FLT x300 = x54 * x140;
	const FLT x301 = (x288 * x135) + x300 + (-1 * x286 * x139) + (-1 * x299) + x298 + (-1 * x285 * x137) + (-1 * x293 * x131) + (-1 * x292 * x131) + (x287 * x136);
	const FLT x302 = (x36 * x289) + x56 + (x37 * x281) + (-1 * x45 * x287) + (-1 * x44 * x288) + (-1 * x34 * x285) + (-1 * x35 * x286);
	const FLT x303 = x283 * x166;
	const FLT x304 = x280 * x144;
	const FLT x305 = (x8 * x304) + (-1 * x51 * x186) + (x293 * x144) + (-1 * x291 * x283) + (x281 * x166) + (x59 * x290) + (x51 * x176) + (-1 * x283 * x165) + x156 + (x6 * x303) + (x297 * x163) + (-1 * x59 * x302) + (-1 * x280 * x165) + (-1 * x301 * x163) + (-1 * x146) + (-1 * x291 * x280);
	const FLT x306 = x63 * x149;
	const FLT x307 = (-1 * x168) + (-1 * x285 * x130) + x169 + (x292 * x139) + (-1 * x170) + (x293 * x139) + (-1 * x281 * x136) + (-1 * x289 * x135) + (-1 * x286 * x131);
	const FLT x308 = 1.0 * x72;
	const FLT x309 = x63 * x308;
	const FLT x310 = x72 * x283;
	const FLT x311 = x280 * x147;
	const FLT x312 = (-1 * x36 * x285) + (-1 * x172) + (-1 * x174) + (x45 * x292) + (-1 * x35 * x281) + (-1 * x37 * x286) + (x45 * x293) + x173 + (-1 * x34 * x289);
	const FLT x313 = (x280 * x183) + (-1 * x312 * x163) + (-1 * x8 * x311) + (x164 * x178) + (-1 * x280 * x184) + (x69 * x302) + (x283 * x183) + (-1 * x293 * x147) + (-1 * x69 * x290) + (-1 * x10 * x303) + x306 + (-1 * x166 * x178) + (x72 * x292) + (x8 * x310) + (x307 * x163) + (-1 * x309);
	const FLT x314 = (x313 * x190) + (x305 * x182);
	const FLT x315 = x6 * x147;
	const FLT x316 = (x51 * x308) + (-1 * x51 * x149);
	const FLT x317 = x179 + x316 + (-1 * x10 * x304) + (-1 * x59 * x307) + (-1 * x6 * x310) + (x69 * x301) + (x283 * x154) + (x280 * x154) + (x6 * x311) + (x283 * x315) + (-1 * x69 * x297) + (-1 * x72 * x281) + (-1 * x283 * x152) + (x59 * x312);
	const FLT x318 = (x222 * x317) + (-1 * x248 * x314);
	const FLT x319 = ((x251 * x305) + (-1 * x250 * x313)) * x252;
	const FLT x320 = (-1 * x319) + (x247 * x318);
	const FLT x321 = (x202 * x317) + (-1 * x200 * (x314 + (x317 * x162)));
	const FLT x322 = x212 * x321;
	const FLT x323 = (x204 * (x322 + (-1 * x211 * x321))) + (x210 * x321);
	const FLT x324 = (x216 * x321) + (x204 * x323);
	const FLT x325 = x319 + (-1 * x270 * (x318 + (-1 * x268 * ((x266 * x320) + (-1 * x235 * ((x204 * x324) + (x264 * x321) + (x258 * x321) + (x204 * (x324 + (x259 * x321) + (x204 * (x323 + (x204 * ((-1 * x262 * x321) + x322 + (x260 * x321))) + (x263 * x321))))))))) + (x241 * x324) + (-1 * x257 * x320) + (x244 * x321)));
	const FLT x326 = x14 * (*_x0).Lighthouse.Rot[2];
	const FLT x327 = x275 * (*_x0).Lighthouse.Rot[1];
	const FLT x328 = x18 * (*_x0).Lighthouse.Rot[0];
	const FLT x329 = x3 * x16;
	const FLT x330 = (-1 * x329) + x328 + x326 + (-1 * x327);
	const FLT x331 = x39 * x330;
	const FLT x332 = x13 * x330;
	const FLT x333 = (-1 * x24 * x329) + (-1 * x4 * x332) + (x24 * x326) + (x24 * x328) + (-1 * x11 * x332) + (-1 * x9 * x332) + (-1 * x24 * x327) + (-1 * x7 * x332);
	const FLT x334 = x31 * x333;
	const FLT x335 = x10 * x334;
	const FLT x336 = x6 * x331;
	const FLT x337 = x10 * x331;
	const FLT x338 = x32 * x333;
	const FLT x339 = x334 * x136;
	const FLT x340 = x331 * x136;
	const FLT x341 = (-1 * x298) + (-1 * x3 * x340) + (-1 * x300) + (-1 * x335 * x139) + (x336 * x131) + (-1 * x337 * x139) + (-1 * x3 * x339) + x299 + (x338 * x131);
	const FLT x342 = x42 * x333;
	const FLT x343 = x3 * x331;
	const FLT x344 = x3 * x334;
	const FLT x345 = x8 * x331;
	const FLT x346 = 2 * (x158 + (x10 * x340) + (-1 * x342 * x131) + (x10 * x339) + (-1 * x345 * x131) + (-1 * x343 * x139) + (-1 * x344 * x139));
	const FLT x347 = x56 + (x35 * x335) + (-1 * x45 * x343) + (-1 * x37 * x345) + (-1 * x37 * x342) + (x35 * x337) + (-1 * x45 * x344);
	const FLT x348 = (x37 * x338) + x294 + (-1 * x35 * x344) + (-1 * x296) + (-1 * x45 * x335) + (-1 * x35 * x343) + (-1 * x295) + (-1 * x45 * x337) + (x37 * x336);
	const FLT x349 = (-1 * x187) + (-1 * x291 * x331) + (-1 * x342 * x153) + (x345 * x144) + (-1 * x331 * x165) + x185 + (x342 * x144) + (-1 * x70 * x346) + (x59 * x341) + (x51 * x145) + (x338 * x166) + (x336 * x166) + (x347 * x163) + (-1 * x334 * x165) + (-1 * x51 * x155) + (-1 * x59 * x348);
	const FLT x350 = (x342 * x139) + (-1 * x343 * x131) + x142 + (-1 * x338 * x136) + x138 + (-1 * x336 * x136) + (x345 * x139) + (-1 * x133) + (-1 * x344 * x131);
	const FLT x351 = (-1 * x37 * x343) + (-1 * x35 * x338) + x65 + (x45 * x342) + (-1 * x60) + (x45 * x345) + (-1 * x37 * x344) + x62 + (-1 * x35 * x336);
	const FLT x352 = x316 + x177 + (x334 * x183) + (-1 * x351 * x163) + (-1 * x331 * x184) + (-1 * x334 * x184) + (x72 * x345) + (-1 * x342 * x147) + (x72 * x342) + (-1 * x69 * x341) + (x350 * x163) + (x69 * x348) + (-1 * x345 * x147) + (x331 * x183);
	const FLT x353 = (x352 * x190) + (x349 * x182);
	const FLT x354 = (-1 * x331 * x152) + (x338 * x147) + (-1 * x59 * x350) + (x331 * x154) + (x315 * x331) + (x144 * x151) + (-1 * x151 * x153) + (-1 * x72 * x336) + x309 + (x334 * x154) + (-1 * x69 * x347) + (-1 * x306) + (x59 * x351) + (-1 * x72 * x338) + (-1 * x334 * x152) + (x68 * x346);
	const FLT x355 = (x222 * x354) + (-1 * x248 * x353);
	const FLT x356 = ((x251 * x349) + (-1 * x250 * x352)) * x252;
	const FLT x357 = (-1 * x356) + (x247 * x355);
	const FLT x358 = (x202 * x354) + (-1 * x200 * (x353 + (x354 * x162)));
	const FLT x359 = x212 * x358;
	const FLT x360 = (x204 * (x359 + (-1 * x211 * x358))) + (x210 * x358);
	const FLT x361 = (x216 * x358) + (x204 * x360);
	const FLT x362 = x356 + (-1 * x270 * (x355 + (-1 * x268 * ((x266 * x357) + (-1 * x235 * ((x204 * (x361 + (x259 * x358) + (x204 * (x360 + (x204 * ((-1 * x262 * x358) + x359 + (x260 * x358))) + (x263 * x358))))) + (x204 * x361) + (x258 * x358) + (x264 * x358))))) + (x241 * x361) + (-1 * x257 * x357) + (x244 * x358)));
	const FLT x363 = 1. / x30;
	const FLT x364 = 2 * x363;
	const FLT x365 = x24 * x364;
	const FLT x366 = x6 * x365;
	const FLT x367 = x3 * x366;
	const FLT x368 = x8 * x365;
	const FLT x369 = x10 * x368;
	const FLT x370 = -1 * x369;
	const FLT x371 = x370 + x367;
	const FLT x372 = x28 * x364;
	const FLT x373 = x27 * x364;
	const FLT x374 = -1 + x373;
	const FLT x375 = x374 + x372;
	const FLT x376 = x10 * x366;
	const FLT x377 = -1 * x376;
	const FLT x378 = x3 * x368;
	const FLT x379 = -1 * x378;
	const FLT x380 = x379 + x377;
	const FLT x381 = (x380 * x190) + (x375 * x182);
	const FLT x382 = (x202 * x371) + (-1 * x200 * (x381 + (x371 * x162)));
	const FLT x383 = x212 * x382;
	const FLT x384 = (x204 * (x383 + (-1 * x211 * x382))) + (x210 * x382);
	const FLT x385 = (x216 * x382) + (x204 * x384);
	const FLT x386 = (x222 * x371) + (-1 * x248 * x381);
	const FLT x387 = ((x251 * x375) + (-1 * x250 * x380)) * x252;
	const FLT x388 = (-1 * x387) + (x247 * x386);
	const FLT x389 = x387 + (-1 * x270 * ((-1 * x268 * ((x266 * x388) + (-1 * x235 * ((x264 * x382) + (x258 * x382) + (x204 * x385) + (x204 * (x385 + (x259 * x382) + (x204 * ((x204 * (x383 + (-1 * x262 * x382) + (x260 * x382))) + x384 + (x263 * x382))))))))) + (-1 * x257 * x388) + (x244 * x382) + x386 + (x241 * x385)));
	const FLT x390 = -1 * x367;
	const FLT x391 = x390 + x370;
	const FLT x392 = x3 * x10;
	const FLT x393 = x392 * x365;
	const FLT x394 = x8 * x366;
	const FLT x395 = -1 * x394;
	const FLT x396 = x395 + x393;
	const FLT x397 = (x396 * x190) + (x391 * x182);
	const FLT x398 = x29 * x364;
	const FLT x399 = x374 + x398;
	const FLT x400 = (x222 * x399) + (-1 * x248 * x397);
	const FLT x401 = ((x251 * x391) + (-1 * x250 * x396)) * x252;
	const FLT x402 = (-1 * x401) + (x400 * x247);
	const FLT x403 = (x202 * x399) + (-1 * x200 * (x397 + (x399 * x162)));
	const FLT x404 = x403 * x212;
	const FLT x405 = (x204 * (x404 + (-1 * x403 * x211))) + (x403 * x210);
	const FLT x406 = (x403 * x216) + (x405 * x204);
	const FLT x407 = x401 + (-1 * x270 * (x400 + (-1 * x268 * ((x402 * x266) + (-1 * x235 * ((x406 * x204) + (x204 * (x406 + (x403 * x259) + (x204 * ((x204 * ((-1 * x403 * x262) + x404 + (x403 * x260))) + x405 + (x403 * x263))))) + (x403 * x258) + (x403 * x264))))) + (x403 * x244) + (-1 * x402 * x257) + (x406 * x241)));
	const FLT x408 = -1 * x393;
	const FLT x409 = x408 + x395;
	const FLT x410 = x377 + x378;
	const FLT x411 = -1 + x372 + x398;
	const FLT x412 = (x411 * x190) + (x410 * x182);
	const FLT x413 = (x409 * x202) + (-1 * x200 * (x412 + (x409 * x162)));
	const FLT x414 = x413 * x212;
	const FLT x415 = (x204 * (x414 + (-1 * x413 * x211))) + (x413 * x210);
	const FLT x416 = (x413 * x216) + (x415 * x204);
	const FLT x417 = (x409 * x222) + (-1 * x412 * x248);
	const FLT x418 = ((x410 * x251) + (-1 * x411 * x250)) * x252;
	const FLT x419 = (-1 * x418) + (x417 * x247);
	const FLT x420 = x418 + (-1 * x270 * ((-1 * x268 * ((x419 * x266) + (-1 * x235 * ((x204 * (x416 + (x413 * x259) + (x204 * (x415 + (x204 * ((-1 * x413 * x262) + x414 + (x413 * x260))) + (x413 * x263))))) + (x416 * x204) + (x413 * x258) + (x413 * x264))))) + (-1 * x419 * x257) + x417 + (x416 * x241) + (x413 * x244)));
	const FLT x421 = x73 * x363;
	const FLT x422 = -1 * x28 * x421;
	const FLT x423 = (-1 * x27 * x421) + x74;
	const FLT x424 = x423 + x422;
	const FLT x425 = x24 * x421;
	const FLT x426 = x8 * x425;
	const FLT x427 = x3 * x426;
	const FLT x428 = x6 * x425;
	const FLT x429 = x10 * x428;
	const FLT x430 = x429 + x427;
	const FLT x431 = (x430 * x190) + (x424 * x182);
	const FLT x432 = x10 * x426;
	const FLT x433 = x3 * x428;
	const FLT x434 = (-1 * x433) + x432;
	const FLT x435 = (x434 * x222) + (-1 * x431 * x248);
	const FLT x436 = ((x424 * x251) + (-1 * x430 * x250)) * x252;
	const FLT x437 = (-1 * x436) + (x435 * x247);
	const FLT x438 = (x434 * x202) + (-1 * x200 * (x431 + (x434 * x162)));
	const FLT x439 = x438 * x212;
	const FLT x440 = (x204 * (x439 + (-1 * x438 * x211))) + (x438 * x210);
	const FLT x441 = (x438 * x216) + (x440 * x204);
	const FLT x442 = x436 + (-1 * x270 * (x435 + (x441 * x241) + (-1 * x268 * ((x437 * x266) + (-1 * x235 * ((x441 * x204) + (x438 * x258) + (x438 * x264) + (x204 * (x441 + (x438 * x259) + (x204 * (x440 + (x204 * ((-1 * x438 * x262) + x439 + (x438 * x260))) + (x438 * x263))))))))) + (-1 * x437 * x257) + (x438 * x244)));
	const FLT x443 = x432 + x433;
	const FLT x444 = x8 * x428;
	const FLT x445 = x425 * x392;
	const FLT x446 = (-1 * x445) + x444;
	const FLT x447 = (x446 * x190) + (x443 * x182);
	const FLT x448 = -1 * x29 * x421;
	const FLT x449 = x423 + x448;
	const FLT x450 = (x449 * x222) + (-1 * x447 * x248);
	const FLT x451 = ((x443 * x251) + (-1 * x446 * x250)) * x252;
	const FLT x452 = (-1 * x451) + (x450 * x247);
	const FLT x453 = (x449 * x202) + (-1 * x200 * (x447 + (x449 * x162)));
	const FLT x454 = x453 * x209;
	const FLT x455 = x453 * x212;
	const FLT x456 = (x204 * (x455 + (-1 * x454 * x205))) + (x453 * x210);
	const FLT x457 = (x453 * x216) + (x456 * x204);
	const FLT x458 = x451 + (-1 * x270 * (x450 + (-1 * x268 * ((x452 * x266) + (-1 * x235 * ((x204 * (x457 + (x453 * x259) + (x204 * (x456 + (x204 * (x455 + (-1 * x454 * x261) + (x453 * x260))) + (x453 * x263))))) + (x457 * x204) + (x453 * x264) + (x453 * x258))))) + (x457 * x241) + (-1 * x452 * x257) + (x454 * x238 * x228 * x243)));
	const FLT x459 = x444 + x445;
	const FLT x460 = (-1 * x427) + x429;
	const FLT x461 = x448 + x422 + x74;
	const FLT x462 = (x461 * x190) + (x460 * x182);
	const FLT x463 = (x459 * x202) + (-1 * x200 * (x462 + (x459 * x162)));
	const FLT x464 = (x459 * x222) + (-1 * x462 * x248);
	const FLT x465 = ((x460 * x251) + (-1 * x461 * x250)) * x252;
	const FLT x466 = (-1 * x465) + (x464 * x247);
	const FLT x467 = x463 * x212;
	const FLT x468 = (x204 * (x467 + (-1 * x463 * x211))) + (x463 * x210);
	const FLT x469 = (x463 * x216) + (x468 * x204);
	const FLT x470 = x465 + (-1 * x270 * ((x469 * x241) + (-1 * x268 * ((x466 * x266) + (-1 * x235 * ((x469 * x204) + (x204 * (x469 + (x463 * x259) + (x204 * (x468 + (x204 * ((-1 * x463 * x262) + x467 + (x463 * x260))) + (x463 * x263))))) + (x463 * x258) + (x463 * x264))))) + (x463 * x244) + x464 + (-1 * x466 * x257)));
	const FLT x471 = 1.0 * (*_x0).Object.Pose.Rot[0];
	const FLT x472 = 1.0 * x102;
	const FLT x473 = 1.0 * x103;
	const FLT x474 = 1.0 * x104;
	const FLT x475 = (x474 * (*_x0).Object.Pose.Rot[3]) + (-1 * x473 * (*_x0).Object.Pose.Rot[2]) + (x471 * x105) + (-1 * x472 * (*_x0).Object.Pose.Rot[1]);
	const FLT x476 = 1. / (x106 * sqrt(x106));
	const FLT x477 = x476 * x112;
	const FLT x478 = 1.0/2.0 * x477;
	const FLT x479 = x478 * x475;
	const FLT x480 = 1.0/2.0 * x102;
	const FLT x481 = x476 * x480;
	const FLT x482 = x98 * x475;
	const FLT x483 = x79 * x109;
	const FLT x484 = 1.0/2.0 * x476;
	const FLT x485 = x484 * x104;
	const FLT x486 = x475 * x485;
	const FLT x487 = x82 * x109;
	const FLT x488 = x475 * x487;
	const FLT x489 = x484 * x488;
	const FLT x490 = x98 * x107;
	const FLT x491 = 0.5 * x490;
	const FLT x492 = x491 * (*_x0).Object.Pose.Rot[1];
	const FLT x493 = 0.5 * x118;
	const FLT x494 = x493 * (*_x0).Object.Pose.Rot[2];
	const FLT x495 = 0.5 * x79;
	const FLT x496 = x495 * x110;
	const FLT x497 = x496 * (*_x0).Object.Pose.Rot[3];
	const FLT x498 = x82 * x110;
	const FLT x499 = 0.5 * x498;
	const FLT x500 = x499 * (*_x0).Object.Pose.Rot[0];
	const FLT x501 = (-1 * x500) + x497 + (-1 * x492) + (-1 * x494);
	const FLT x502 = (x489 * x105) + (-1 * x483 * x486) + x501 + (-1 * x479 * x103) + (-1 * x482 * x481);
	const FLT x503 = 2 * x125;
	const FLT x504 = x484 * x103;
	const FLT x505 = x475 * x483;
	const FLT x506 = x496 * (*_x0).Object.Pose.Rot[2];
	const FLT x507 = x499 * (*_x0).Object.Pose.Rot[1];
	const FLT x508 = x484 * x105;
	const FLT x509 = x491 * (*_x0).Object.Pose.Rot[0];
	const FLT x510 = x493 * (*_x0).Object.Pose.Rot[3];
	const FLT x511 = (-1 * x479 * x104) + x509 + (-1 * x481 * x488) + x510 + (-1 * x507) + (x504 * x505) + x506 + (-1 * x482 * x508);
	const FLT x512 = x477 * x480;
	const FLT x513 = x98 * x504;
	const FLT x514 = x496 * (*_x0).Object.Pose.Rot[0];
	const FLT x515 = x491 * (*_x0).Object.Pose.Rot[2];
	const FLT x516 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x517 = x498 * x516;
	const FLT x518 = x493 * (*_x0).Object.Pose.Rot[1];
	const FLT x519 = (-1 * x518) + (-1 * x517) + (-1 * x514) + x515;
	const FLT x520 = x519 + (x488 * x485) + (x475 * x513) + (x505 * x508) + (-1 * x475 * x512);
	const FLT x521 = (-1 * x496 * (*_x0).Object.Pose.Rot[1]) + (x493 * (*_x0).Object.Pose.Rot[0]) + (-1 * x490 * x516) + (-1 * x499 * (*_x0).Object.Pose.Rot[2]);
	const FLT x522 = x521 + (-1 * x479 * x105) + (-1 * x489 * x103) + (x98 * x486) + (-1 * x481 * x505);
	const FLT x523 = (x522 * sensor_pt[1]) + (-1 * x511 * sensor_pt[0]) + (x520 * sensor_pt[2]);
	const FLT x524 = 2 * x128;
	const FLT x525 = (-1 * x522 * sensor_pt[2]) + (x502 * sensor_pt[0]) + (x520 * sensor_pt[1]);
	const FLT x526 = 2 * x117;
	const FLT x527 = 2 * x129;
	const FLT x528 = (-1 * x525 * x526) + (-1 * x502 * x503) + (x511 * x527) + (x524 * x523);
	const FLT x529 = 2 * x134;
	const FLT x530 = (x511 * sensor_pt[2]) + (-1 * x502 * sensor_pt[1]) + (x520 * sensor_pt[0]);
	const FLT x531 = 2 * x124;
	const FLT x532 = (x525 * x531) + (x522 * x503) + (-1 * x511 * x529) + (-1 * x524 * x530);
	const FLT x533 = (-1 * x523 * x531) + (x529 * x502) + (-1 * x522 * x527) + (x526 * x530);
	const FLT x534 = (-1 * x68 * x533) + (x70 * x528) + (x71 * x532);
	const FLT x535 = (x58 * x533) + (-1 * x70 * x532) + (x71 * x528);
	const FLT x536 = x533 + (x69 * x534) + (-1 * x59 * x535);
	const FLT x537 = (-1 * x58 * x528) + (x71 * x533) + (x68 * x532);
	const FLT x538 = (x59 * x537) + x528 + (-1 * x534 * x163);
	const FLT x539 = (x535 * x163) + x532 + (-1 * x69 * x537);
	const FLT x540 = (x539 * x190) + (x538 * x182);
	const FLT x541 = (x536 * x202) + (-1 * x200 * (x540 + (x536 * x162)));
	const FLT x542 = x541 * x212;
	const FLT x543 = (x204 * (x542 + (-1 * x541 * x211))) + (x541 * x210);
	const FLT x544 = (x541 * x216) + (x543 * x204);
	const FLT x545 = (x536 * x222) + (-1 * x540 * x248);
	const FLT x546 = ((x538 * x251) + (-1 * x539 * x250)) * x252;
	const FLT x547 = (-1 * x546) + (x545 * x247);
	const FLT x548 = x546 + (-1 * x270 * (x545 + (-1 * x547 * x257) + (-1 * x268 * ((x547 * x266) + (-1 * x235 * ((x544 * x204) + (x541 * x258) + (x541 * x264) + (x204 * (x544 + (x541 * x259) + (x204 * (x543 + (x204 * ((-1 * x541 * x262) + x542 + (x541 * x260))) + (x541 * x263))))))))) + (x541 * x244) + (x544 * x241)));
	const FLT x549 = 1.0 * x105;
	const FLT x550 = (x471 * x104) + (-1 * x472 * (*_x0).Object.Pose.Rot[2]) + (-1 * x549 * (*_x0).Object.Pose.Rot[3]) + (x473 * (*_x0).Object.Pose.Rot[1]);
	const FLT x551 = x483 * x550;
	const FLT x552 = x98 * x550;
	const FLT x553 = x487 * x550;
	const FLT x554 = x478 * x550;
	const FLT x555 = x521 + (-1 * x554 * x104) + (-1 * x481 * x553) + (x504 * x551) + (-1 * x508 * x552);
	const FLT x556 = x507 + (-1 * x509) + (-1 * x510) + (-1 * x506);
	const FLT x557 = x556 + (-1 * x504 * x553) + (-1 * x554 * x105) + (-1 * x481 * x551) + (x485 * x552);
	const FLT x558 = (x485 * x553) + (-1 * x512 * x550) + (x508 * x551) + x501 + (x504 * x552);
	const FLT x559 = (-1 * x515) + (-1 * x554 * x103) + x517 + (x508 * x553) + (-1 * x485 * x551) + x518 + (-1 * x481 * x552) + x514;
	const FLT x560 = (x559 * sensor_pt[0]) + (-1 * x557 * sensor_pt[2]) + (x558 * sensor_pt[1]);
	const FLT x561 = (-1 * x559 * sensor_pt[1]) + (x555 * sensor_pt[2]) + (x558 * sensor_pt[0]);
	const FLT x562 = (x503 * x557) + (-1 * x524 * x561) + (-1 * x529 * x555) + (x531 * x560);
	const FLT x563 = (x557 * sensor_pt[1]) + (-1 * x555 * sensor_pt[0]) + (x558 * sensor_pt[2]);
	const FLT x564 = (x526 * x561) + (-1 * x531 * x563) + (-1 * x527 * x557) + (x529 * x559);
	const FLT x565 = (x524 * x563) + (x527 * x555) + (-1 * x503 * x559) + (-1 * x526 * x560);
	const FLT x566 = (-1 * x58 * x565) + (x68 * x562) + (x71 * x564);
	const FLT x567 = (x71 * x562) + (-1 * x68 * x564) + (x70 * x565);
	const FLT x568 = x565 + (x59 * x566) + (-1 * x567 * x163);
	const FLT x569 = (-1 * x70 * x562) + (x58 * x564) + (x71 * x565);
	const FLT x570 = x562 + (x569 * x163) + (-1 * x69 * x566);
	const FLT x571 = (x570 * x190) + (x568 * x182);
	const FLT x572 = (x69 * x567) + x564 + (-1 * x59 * x569);
	const FLT x573 = (x572 * x222) + (-1 * x571 * x248);
	const FLT x574 = ((x568 * x251) + (-1 * x570 * x250)) * x252;
	const FLT x575 = (-1 * x574) + (x573 * x247);
	const FLT x576 = (x572 * x202) + (-1 * x200 * (x571 + (x572 * x162)));
	const FLT x577 = x576 * x212;
	const FLT x578 = (x204 * (x577 + (-1 * x576 * x211))) + (x576 * x210);
	const FLT x579 = (x576 * x216) + (x578 * x204);
	const FLT x580 = x574 + (-1 * x270 * (x573 + (-1 * x268 * ((x575 * x266) + (-1 * x235 * ((x579 * x204) + (x576 * x264) + (x576 * x258) + (x204 * (x579 + (x576 * x259) + (x204 * (x578 + (x204 * ((-1 * x576 * x262) + x577 + (x576 * x260))) + (x576 * x263))))))))) + (x579 * x241) + (-1 * x575 * x257) + (x576 * x244)));
	const FLT x581 = (-1 * x474 * (*_x0).Object.Pose.Rot[1]) + (x549 * (*_x0).Object.Pose.Rot[2]) + (-1 * x472 * (*_x0).Object.Pose.Rot[3]) + (x471 * x103);
	const FLT x582 = x98 * x581;
	const FLT x583 = x483 * x581;
	const FLT x584 = x487 * x581;
	const FLT x585 = x478 * x581;
	const FLT x586 = (-1 * x585 * x104) + x519 + (-1 * x481 * x584) + (-1 * x508 * x582) + (x504 * x583);
	const FLT x587 = (x508 * x584) + (-1 * x585 * x103) + x521 + (-1 * x481 * x582) + (-1 * x485 * x583);
	const FLT x588 = x556 + (x513 * x581) + (x485 * x584) + (-1 * x512 * x581) + (x508 * x583);
	const FLT x589 = (x586 * sensor_pt[2]) + (-1 * x587 * sensor_pt[1]) + (x588 * sensor_pt[0]);
	const FLT x590 = (-1 * x504 * x584) + x500 + (-1 * x585 * x105) + x492 + (-1 * x497) + (-1 * x481 * x583) + (x485 * x582) + x494;
	const FLT x591 = (x587 * sensor_pt[0]) + (-1 * x590 * sensor_pt[2]) + (x588 * sensor_pt[1]);
	const FLT x592 = (x590 * x503) + (x591 * x531) + (-1 * x529 * x586) + (-1 * x524 * x589);
	const FLT x593 = (x590 * sensor_pt[1]) + (x588 * sensor_pt[2]) + (-1 * x586 * sensor_pt[0]);
	const FLT x594 = (x526 * x589) + (-1 * x590 * x527) + (x529 * x587) + (-1 * x593 * x531);
	const FLT x595 = (x593 * x524) + (x527 * x586) + (-1 * x503 * x587) + (-1 * x591 * x526);
	const FLT x596 = (-1 * x58 * x595) + (x68 * x592) + (x71 * x594);
	const FLT x597 = x50 * x595;
	const FLT x598 = x50 * x592;
	const FLT x599 = (x8 * x597) + (-1 * x68 * x594) + (x3 * x598);
	const FLT x600 = (x59 * x596) + x595 + (-1 * x599 * x163);
	const FLT x601 = (-1 * x8 * x598) + (x58 * x594) + (x3 * x597);
	const FLT x602 = x592 + (x601 * x163) + (-1 * x69 * x596);
	const FLT x603 = (x602 * x190) + (x600 * x182);
	const FLT x604 = x594 + (x69 * x599) + (-1 * x59 * x601);
	const FLT x605 = (x604 * x222) + (-1 * x603 * x248);
	const FLT x606 = ((x600 * x251) + (-1 * x602 * x250)) * x252;
	const FLT x607 = (-1 * x606) + (x605 * x247);
	const FLT x608 = (x604 * x202) + (-1 * x200 * (x603 + (x604 * x162)));
	const FLT x609 = x608 * x212;
	const FLT x610 = (x204 * (x609 + (-1 * x608 * x211))) + (x608 * x210);
	const FLT x611 = (x608 * x216) + (x610 * x204);
	const FLT x612 = x606 + (-1 * x270 * ((-1 * x268 * ((x607 * x266) + (-1 * x235 * ((x611 * x204) + (x608 * x258) + (x608 * x264) + (x204 * (x611 + (x608 * x259) + (x204 * ((x204 * ((-1 * x608 * x262) + x609 + (x608 * x260))) + x610 + (x608 * x263))))))))) + (x611 * x241) + x605 + (-1 * x607 * x257) + (x608 * x244)));
	const FLT x613 = -1 * x373;
	const FLT x614 = 1 + (-1 * x372);
	const FLT x615 = x614 + x613;
	const FLT x616 = x376 + x378;
	const FLT x617 = (x616 * x190) + (x615 * x182);
	const FLT x618 = x390 + x369;
	const FLT x619 = (x618 * x222) + (-1 * x617 * x248);
	const FLT x620 = ((x615 * x251) + (-1 * x616 * x250)) * x252;
	const FLT x621 = (-1 * x620) + (x619 * x247);
	const FLT x622 = (x618 * x202) + (-1 * x200 * (x617 + (x618 * x162)));
	const FLT x623 = x622 * x212;
	const FLT x624 = (x204 * (x623 + (-1 * x622 * x211))) + (x622 * x210);
	const FLT x625 = (x622 * x216) + (x624 * x204);
	const FLT x626 = x620 + (-1 * x270 * (x619 + (x625 * x241) + (-1 * x621 * x257) + (-1 * x268 * ((x621 * x266) + (-1 * x235 * ((x622 * x264) + (x622 * x258) + (x625 * x204) + (x204 * (x625 + (x622 * x259) + (x204 * ((x204 * ((-1 * x622 * x262) + x623 + (x622 * x260))) + x624 + (x622 * x263))))))))) + (x622 * x244)));
	const FLT x627 = x369 + x367;
	const FLT x628 = x408 + x394;
	const FLT x629 = (x628 * x190) + (x627 * x182);
	const FLT x630 = -1 * x398;
	const FLT x631 = 1 + x613 + x630;
	const FLT x632 = (x631 * x222) + (-1 * x629 * x248);
	const FLT x633 = ((x627 * x251) + (-1 * x628 * x250)) * x252;
	const FLT x634 = (-1 * x633) + (x632 * x247);
	const FLT x635 = (x631 * x202) + (-1 * x200 * (x629 + (x631 * x162)));
	const FLT x636 = x635 * x212;
	const FLT x637 = (x204 * (x636 + (-1 * x635 * x211))) + (x635 * x210);
	const FLT x638 = (x635 * x216) + (x637 * x204);
	const FLT x639 = x633 + (-1 * x270 * (x632 + (x638 * x241) + (x635 * x244) + (-1 * x634 * x257) + (-1 * x268 * ((x634 * x266) + (-1 * x235 * ((x638 * x204) + (x635 * x264) + (x635 * x258) + (x204 * ((x635 * x259) + x638 + (x204 * ((x204 * ((-1 * x635 * x262) + (x635 * x260) + x636)) + x637 + (x635 * x263)))))))))));
	const FLT x640 = x379 + x376;
	const FLT x641 = x614 + x630;
	const FLT x642 = (x641 * x190) + (x640 * x182);
	const FLT x643 = x394 + x393;
	const FLT x644 = (x643 * x222) + (-1 * x642 * x248);
	const FLT x645 = ((x640 * x251) + (-1 * x641 * x250)) * x252;
	const FLT x646 = (-1 * x645) + (x644 * x247);
	const FLT x647 = (x643 * x202) + (-1 * x200 * (x642 + (x643 * x162)));
	const FLT x648 = x647 * x212;
	const FLT x649 = (x204 * (x648 + (-1 * x647 * x211))) + (x647 * x210);
	const FLT x650 = (x647 * x216) + (x649 * x204);
	const FLT x651 = x645 + (-1 * x270 * ((-1 * x268 * ((x646 * x266) + (-1 * x235 * ((x650 * x204) + (x647 * x264) + (x204 * (x650 + (x647 * x259) + (x204 * (x649 + (x204 * ((-1 * x647 * x262) + (x647 * x260) + x648)) + (x647 * x263))))) + (x647 * x258))))) + (x650 * x241) + (-1 * x646 * x257) + x644 + (x647 * x244)));
	const FLT x652 = -1 * x121;
	const FLT x653 = x89 * x495;
	const FLT x654 = x96 * x115;
	const FLT x655 = x76 * x654;
	const FLT x656 = dt * dt * dt;
	const FLT x657 = 0.5 * x92;
	const FLT x658 = x657 * x656;
	const FLT x659 = x80 * x658;
	const FLT x660 = x480 * x107;
	const FLT x661 = x79 * x79 * x79;
	const FLT x662 = dt * dt * dt * dt;
	const FLT x663 = (1. / (x85 * x85)) * x91;
	const FLT x664 = 2 * x663;
	const FLT x665 = x664 * x662;
	const FLT x666 = x77 * x662;
	const FLT x667 = x88 * (1. / (x85 * sqrt(x85)));
	const FLT x668 = 1.0 * x94;
	const FLT x669 = x668 * x667;
	const FLT x670 = x79 * x669;
	const FLT x671 = x83 * x662;
	const FLT x672 = 2 * x79;
	const FLT x673 = x672 * x663;
	const FLT x674 = x76 * x93;
	const FLT x675 = x669 * x662;
	const FLT x676 = x76 * x668;
	const FLT x677 = x89 * x676;
	const FLT x678 = (x671 * x670) + (-1 * x79 * x677) + (x670 * x666) + (x672 * x674) + (x675 * x661) + (-1 * x673 * x666) + (-1 * x661 * x665) + (-1 * x671 * x673);
	const FLT x679 = x82 * x89;
	const FLT x680 = 1. / (x95 * sqrt(x95));
	const FLT x681 = dt * x680;
	const FLT x682 = x679 * x681;
	const FLT x683 = x678 * x682;
	const FLT x684 = 1.0/2.0 * x127;
	const FLT x685 = x678 * x681;
	const FLT x686 = x90 * x685;
	const FLT x687 = x94 * x680;
	const FLT x688 = 1.0/2.0 * x687;
	const FLT x689 = x678 * x688;
	const FLT x690 = x96 * x127;
	const FLT x691 = x656 * x667;
	const FLT x692 = x79 * x75;
	const FLT x693 = x692 * x691;
	const FLT x694 = x693 * x690;
	const FLT x695 = x96 * x113;
	const FLT x696 = x80 * x691;
	const FLT x697 = x692 * x658;
	const FLT x698 = x697 * x122;
	const FLT x699 = x89 * x79;
	const FLT x700 = x699 * x685;
	const FLT x701 = 1.0/2.0 * x113;
	const FLT x702 = x96 * x108;
	const FLT x703 = x82 * x691;
	const FLT x704 = x79 * x703;
	const FLT x705 = x82 * x658;
	const FLT x706 = x79 * x705;
	const FLT x707 = (x706 * x119) + (-1 * x702 * x704);
	const FLT x708 = x707 + (x700 * x701) + (x696 * x695) + x698 + (-1 * x689 * x115) + (-1 * x653 * x655) + (-1 * x659 * x114) + (-1 * x694) + (-1 * x683 * x660) + x652 + (-1 * x684 * x686);
	const FLT x709 = 1.0/2.0 * x115;
	const FLT x710 = x76 * x695;
	const FLT x711 = (-1 * x693 * x702) + (x697 * x119);
	const FLT x712 = x690 * x703;
	const FLT x713 = (-1 * x706 * x122) + (x79 * x712);
	const FLT x714 = x713 + x711 + (-1 * x689 * x113) + (-1 * x653 * x710) + (x659 * x126) + (-1 * x696 * x654) + (-1 * x686 * x660) + (x684 * x683) + x116 + (-1 * x709 * x700);
	const FLT x715 = x654 * x704;
	const FLT x716 = x705 * x126;
	const FLT x717 = x79 * x716;
	const FLT x718 = x76 * x702;
	const FLT x719 = -1 * x111;
	const FLT x720 = x480 * x687;
	const FLT x721 = x720 * x107;
	const FLT x722 = (x695 * x693) + (-1 * x697 * x114);
	const FLT x723 = (x684 * x700) + (-1 * x678 * x721) + (-1 * x659 * x122) + (x696 * x690) + x719 + x715 + (-1 * x717) + x722 + (-1 * x653 * x718) + (x683 * x709) + (x686 * x701);
	const FLT x724 = (x708 * sensor_pt[2]) + (-1 * x714 * sensor_pt[1]) + (x723 * sensor_pt[0]);
	const FLT x725 = x697 * x126;
	const FLT x726 = x706 * x114;
	const FLT x727 = x76 * x690;
	const FLT x728 = x695 * x704;
	const FLT x729 = x693 * x654;
	const FLT x730 = x726 + (-1 * x660 * x700) + x123 + (-1 * x653 * x727) + (-1 * x683 * x701) + (x659 * x119) + (-1 * x725) + (-1 * x696 * x702) + (-1 * x689 * x127) + (x686 * x709) + (-1 * x728) + x729;
	const FLT x731 = 2 * ((x714 * sensor_pt[0]) + (-1 * x730 * sensor_pt[2]) + (x723 * sensor_pt[1]));
	const FLT x732 = (x731 * x124) + (x730 * x503) + (-1 * x708 * x529) + (-1 * x724 * x524);
	const FLT x733 = (x730 * sensor_pt[1]) + (-1 * x708 * sensor_pt[0]) + (x723 * sensor_pt[2]);
	const FLT x734 = (-1 * x733 * x531) + (x724 * x526) + (-1 * x730 * x527) + (x714 * x529);
	const FLT x735 = (-1 * x731 * x117) + (x708 * x527) + (-1 * x714 * x503) + (x733 * x524);
	const FLT x736 = (x68 * x732) + (-1 * x58 * x735) + (x71 * x734);
	const FLT x737 = (x70 * x735) + (-1 * x68 * x734) + (x71 * x732);
	const FLT x738 = x735 + (x59 * x736) + (-1 * x737 * x163);
	const FLT x739 = (-1 * x70 * x732) + (x58 * x734) + (x71 * x735);
	const FLT x740 = x732 + (x739 * x163) + (-1 * x69 * x736);
	const FLT x741 = (x740 * x190) + (x738 * x182);
	const FLT x742 = (x69 * x737) + x734 + (-1 * x59 * x739);
	const FLT x743 = (x742 * x222) + (-1 * x741 * x248);
	const FLT x744 = ((x738 * x251) + (-1 * x740 * x250)) * x252;
	const FLT x745 = (-1 * x744) + (x743 * x247);
	const FLT x746 = (x742 * x202) + (-1 * x200 * (x741 + (x742 * x162)));
	const FLT x747 = x746 * x212;
	const FLT x748 = (x204 * (x747 + (-1 * x746 * x211))) + (x746 * x210);
	const FLT x749 = (x746 * x216) + (x748 * x204);
	const FLT x750 = x744 + (-1 * x270 * (x743 + (-1 * x268 * ((x745 * x266) + (-1 * x235 * ((x746 * x264) + (x746 * x258) + (x749 * x204) + (x204 * (x749 + (x746 * x259) + (x204 * (x748 + (x204 * ((-1 * x746 * x262) + x747 + (x746 * x260))) + (x746 * x263))))))))) + (x749 * x241) + (-1 * x745 * x257) + (x746 * x244)));
	const FLT x751 = x83 * x691;
	const FLT x752 = x96 * x751;
	const FLT x753 = x75 * x712;
	const FLT x754 = x83 * x658;
	const FLT x755 = x80 * x665;
	const FLT x756 = x80 * x675;
	const FLT x757 = x82 * x82 * x82;
	const FLT x758 = 2 * x674;
	const FLT x759 = (-1 * x665 * x757) + (x82 * x756) + (x675 * x757) + (-1 * x82 * x677) + (-1 * x82 * x755) + (-1 * x82 * x77 * x665) + (x82 * x758) + (x82 * x669 * x666);
	const FLT x760 = x759 * x107;
	const FLT x761 = x682 * x760;
	const FLT x762 = x688 * x759;
	const FLT x763 = x90 * x681;
	const FLT x764 = 1.0/2.0 * x105;
	const FLT x765 = x764 * x760;
	const FLT x766 = 0.5 * x679;
	const FLT x767 = x75 * x705;
	const FLT x768 = x767 * x122;
	const FLT x769 = x699 * x681;
	const FLT x770 = x769 * x759;
	const FLT x771 = (x770 * x701) + (-1 * x753) + x123 + (x754 * x119) + (-1 * x726) + (-1 * x765 * x763) + (-1 * x655 * x766) + (-1 * x752 * x108) + x728 + (-1 * x480 * x761) + (-1 * x762 * x115) + x768;
	const FLT x772 = x480 * x760;
	const FLT x773 = x763 * x759;
	const FLT x774 = x682 * x759;
	const FLT x775 = x688 * x105;
	const FLT x776 = x75 * x703;
	const FLT x777 = (-1 * x75 * x716) + (x654 * x776);
	const FLT x778 = x777 + x707 + (-1 * x766 * x727) + x121 + (x754 * x114) + (-1 * x760 * x775) + (-1 * x774 * x701) + (-1 * x695 * x751) + (-1 * x769 * x772) + (x773 * x709);
	const FLT x779 = -1 * x116;
	const FLT x780 = x695 * x776;
	const FLT x781 = x767 * x114;
	const FLT x782 = (-1 * x766 * x718) + (x773 * x701) + (-1 * x754 * x126) + (x769 * x765) + x779 + (-1 * x760 * x720) + (x654 * x751) + x713 + (x774 * x709) + (-1 * x781) + x780;
	const FLT x783 = (-1 * x776 * x702) + (x767 * x119);
	const FLT x784 = (-1 * x766 * x710) + (x764 * x761) + (-1 * x763 * x772) + (-1 * x762 * x113) + (-1 * x754 * x122) + x783 + (-1 * x715) + x719 + (-1 * x770 * x709) + (x752 * x127) + x717;
	const FLT x785 = (x784 * sensor_pt[0]) + (-1 * x778 * sensor_pt[2]) + (x782 * sensor_pt[1]);
	const FLT x786 = (x771 * sensor_pt[2]) + (-1 * x784 * sensor_pt[1]) + (x782 * sensor_pt[0]);
	const FLT x787 = (-1 * x786 * x524) + (x785 * x531) + (-1 * x771 * x529) + (x778 * x503);
	const FLT x788 = (x778 * sensor_pt[1]) + (-1 * x771 * sensor_pt[0]) + (x782 * sensor_pt[2]);
	const FLT x789 = (-1 * x788 * x531) + (x786 * x526) + (-1 * x778 * x527) + (x784 * x529);
	const FLT x790 = (-1 * x785 * x526) + (-1 * x784 * x503) + (x771 * x527) + (x788 * x524);
	const FLT x791 = (x68 * x787) + (-1 * x58 * x790) + (x71 * x789);
	const FLT x792 = (-1 * x68 * x789) + (x70 * x790) + (x71 * x787);
	const FLT x793 = x790 + (x59 * x791) + (-1 * x792 * x163);
	const FLT x794 = (-1 * x70 * x787) + (x71 * x790) + (x58 * x789);
	const FLT x795 = x787 + (x794 * x163) + (-1 * x69 * x791);
	const FLT x796 = (x795 * x190) + (x793 * x182);
	const FLT x797 = x789 + (-1 * x59 * x794) + (x69 * x792);
	const FLT x798 = (x797 * x222) + (-1 * x796 * x248);
	const FLT x799 = ((x793 * x251) + (-1 * x795 * x250)) * x252;
	const FLT x800 = (-1 * x799) + (x798 * x247);
	const FLT x801 = (x797 * x202) + (-1 * x200 * (x796 + (x797 * x162)));
	const FLT x802 = x801 * x212;
	const FLT x803 = (x204 * (x802 + (-1 * x801 * x211))) + (x801 * x210);
	const FLT x804 = (x801 * x216) + (x803 * x204);
	const FLT x805 = x799 + (-1 * x270 * (x798 + (-1 * x268 * ((x800 * x266) + (-1 * x235 * ((x804 * x204) + (x801 * x264) + (x801 * x258) + (x204 * (x804 + (x801 * x259) + (x204 * (x803 + (x204 * ((-1 * x801 * x262) + x802 + (x801 * x260))) + (x801 * x263))))))))) + (x804 * x241) + (-1 * x800 * x257) + (x801 * x244)));
	const FLT x806 = (x75 * x75 * x75) * x662;
	const FLT x807 = (x669 * x806) + (x75 * x671 * x669) + (-1 * x664 * x806) + (-1 * x75 * x755) + (x75 * x756) + (x75 * x758) + (-1 * x83 * x75 * x665) + (-1 * x90 * x676);
	const FLT x808 = x807 * x107;
	const FLT x809 = x480 * x808;
	const FLT x810 = x77 * x656;
	const FLT x811 = x667 * x810;
	const FLT x812 = x96 * x811;
	const FLT x813 = x682 * x807;
	const FLT x814 = x657 * x810;
	const FLT x815 = 0.5 * x90;
	const FLT x816 = x688 * x807;
	const FLT x817 = x769 * x807;
	const FLT x818 = (-1 * x709 * x817) + x123 + (-1 * x812 * x108) + x725 + (x684 * x813) + (-1 * x763 * x809) + x753 + (x814 * x119) + (-1 * x710 * x815) + (-1 * x768) + (-1 * x816 * x113) + (-1 * x729);
	const FLT x819 = x763 * x807;
	const FLT x820 = (x709 * x819) + (-1 * x769 * x809) + x779 + x781 + (-1 * x780) + (-1 * x775 * x808) + (x654 * x811) + x711 + (-1 * x701 * x813) + (-1 * x727 * x815) + (-1 * x814 * x126);
	const FLT x821 = x777 + (-1 * x721 * x807) + (-1 * x718 * x815) + (x709 * x813) + (x812 * x113) + (x684 * x817) + (-1 * x698) + (-1 * x814 * x114) + x694 + x652 + (x701 * x819);
	const FLT x822 = (-1 * x820 * sensor_pt[2]) + (x818 * sensor_pt[0]) + (x821 * sensor_pt[1]);
	const FLT x823 = x722 + (-1 * x812 * x127) + (-1 * x655 * x815) + (x814 * x122) + (-1 * x816 * x115) + (x701 * x817) + x783 + (-1 * x682 * x809) + x111 + (-1 * x684 * x819);
	const FLT x824 = (x820 * sensor_pt[1]) + (x821 * sensor_pt[2]) + (-1 * x823 * sensor_pt[0]);
	const FLT x825 = (x524 * x824) + (x527 * x823) + (-1 * x503 * x818) + (-1 * x526 * x822);
	const FLT x826 = (x823 * sensor_pt[2]) + (-1 * x818 * sensor_pt[1]) + (x821 * sensor_pt[0]);
	const FLT x827 = (-1 * x524 * x826) + (x531 * x822) + (-1 * x529 * x823) + (x503 * x820);
	const FLT x828 = (x526 * x826) + (-1 * x531 * x824) + (-1 * x527 * x820) + (x529 * x818);
	const FLT x829 = (-1 * x68 * x828) + (x70 * x825) + (x71 * x827);
	const FLT x830 = (-1 * x70 * x827) + (x58 * x828) + (x71 * x825);
	const FLT x831 = x828 + (x69 * x829) + (-1 * x59 * x830);
	const FLT x832 = (-1 * x58 * x825) + (x68 * x827) + (x71 * x828);
	const FLT x833 = x825 + (x59 * x832) + (-1 * x829 * x163);
	const FLT x834 = x827 + (x830 * x163) + (-1 * x69 * x832);
	const FLT x835 = (x834 * x190) + (x833 * x182);
	const FLT x836 = (x831 * x202) + (-1 * x200 * (x835 + (x831 * x162)));
	const FLT x837 = x836 * x212;
	const FLT x838 = (x204 * (x837 + (-1 * x836 * x211))) + (x836 * x210);
	const FLT x839 = (x836 * x216) + (x838 * x204);
	const FLT x840 = (x831 * x222) + (-1 * x835 * x248);
	const FLT x841 = ((x833 * x251) + (-1 * x834 * x250)) * x252;
	const FLT x842 = (-1 * x841) + (x840 * x247);
	const FLT x843 = x841 + (-1 * x270 * (x840 + (-1 * x842 * x257) + (-1 * x268 * ((x842 * x266) + (-1 * x235 * ((x839 * x204) + (x836 * x258) + (x836 * x264) + (x204 * (x839 + (x836 * x259) + (x204 * (x838 + (x204 * ((-1 * x836 * x262) + x837 + (x836 * x260))) + (x836 * x263))))))))) + (x836 * x244) + (x839 * x241)));
	const FLT x844 = -1 * dt * x372;
	const FLT x845 = -1 * dt * x373;
	const FLT x846 = x845 + x844 + dt;
	const FLT x847 = dt * x378;
	const FLT x848 = dt * x376;
	const FLT x849 = x848 + x847;
	const FLT x850 = (x849 * x190) + (x846 * x182);
	const FLT x851 = dt * x369;
	const FLT x852 = dt * x367;
	const FLT x853 = (-1 * x852) + x851;
	const FLT x854 = (x853 * x222) + (-1 * x850 * x248);
	const FLT x855 = ((x846 * x251) + (-1 * x849 * x250)) * x252;
	const FLT x856 = (-1 * x855) + (x854 * x247);
	const FLT x857 = (x853 * x202) + (-1 * x200 * (x850 + (x853 * x162)));
	const FLT x858 = x857 * x212;
	const FLT x859 = (x204 * (x858 + (-1 * x857 * x211))) + (x857 * x210);
	const FLT x860 = (x857 * x216) + (x859 * x204);
	const FLT x861 = x855 + (-1 * x270 * ((-1 * x268 * ((x856 * x266) + (-1 * x235 * ((x860 * x204) + (x204 * (x860 + (x857 * x259) + (x204 * (x859 + (x204 * ((-1 * x857 * x262) + x858 + (x857 * x260))) + (x857 * x263))))) + (x857 * x264) + (x857 * x258))))) + x854 + (x860 * x241) + (-1 * x856 * x257) + (x857 * x244)));
	const FLT x862 = (-1 * dt * x398) + dt;
	const FLT x863 = x862 + x845;
	const FLT x864 = x851 + x852;
	const FLT x865 = dt * x394;
	const FLT x866 = dt * x393;
	const FLT x867 = (-1 * x866) + x865;
	const FLT x868 = (x867 * x190) + (x864 * x182);
	const FLT x869 = (x863 * x202) + (-1 * x200 * (x868 + (x863 * x162)));
	const FLT x870 = x869 * x212;
	const FLT x871 = (x204 * (x870 + (-1 * x869 * x211))) + (x869 * x210);
	const FLT x872 = (x869 * x216) + (x871 * x204);
	const FLT x873 = (x863 * x222) + (-1 * x868 * x248);
	const FLT x874 = ((x864 * x251) + (-1 * x867 * x250)) * x252;
	const FLT x875 = x256 * ((-1 * x874) + (x873 * x247));
	const FLT x876 = x874 + (-1 * x270 * (x873 + (-1 * x875 * x255) + (-1 * x268 * ((x875 * x265) + (-1 * x235 * ((x872 * x204) + (x869 * x264) + (x869 * x258) + (x204 * ((x869 * x259) + x872 + (x204 * (x871 + (x204 * ((-1 * x869 * x262) + x870 + (x869 * x260))) + (x869 * x263))))))))) + (x869 * x244) + (x872 * x241)));
	const FLT x877 = (-1 * x847) + x848;
	const FLT x878 = x862 + x844;
	const FLT x879 = (x878 * x190) + (x877 * x182);
	const FLT x880 = x865 + x866;
	const FLT x881 = (x880 * x222) + (-1 * x879 * x248);
	const FLT x882 = ((x877 * x251) + (-1 * x878 * x250)) * x252;
	const FLT x883 = (-1 * x882) + (x881 * x247);
	const FLT x884 = (x880 * x202) + (-1 * x200 * (x879 + (x880 * x162)));
	const FLT x885 = x884 * x212;
	const FLT x886 = (x204 * (x885 + (-1 * x884 * x211))) + (x884 * x210);
	const FLT x887 = (x884 * x216) + (x886 * x204);
	const FLT x888 = x882 + (-1 * x270 * (x881 + (x884 * x244) + (-1 * x883 * x257) + (-1 * x268 * ((x883 * x266) + (-1 * x235 * ((x887 * x204) + (x884 * x264) + (x884 * x258) + (x204 * (x887 + (x884 * x259) + (x204 * ((x204 * (x885 + (-1 * x884 * x262) + (x884 * x260))) + x886 + (x884 * x263))))))))) + (x887 * x241)));
	const FLT x889 = x236 * x267;
	const FLT x890 = (x255 + x889) * x270;
	const FLT x891 = ((-1 * x255 * x225) + (-1 * x889 * x225)) * x270;
	const FLT x892 = x270 * (x257 + (x889 * x256));
	const FLT x893 = x220 * x161 * (1 + x246);
	const FLT x894 = x201 * x208;
	const FLT x895 = x894 * x234 * x161;
	const FLT x896 = x895 * x212;
	const FLT x897 = (x204 * (x896 + (-1 * x895 * x211))) + (x895 * x210);
	const FLT x898 = (x895 * x216) + (x897 * x204);
	const FLT x899 = x893 * x247;
	const FLT x900 = x270 * ((-1 * x899 * x257) + (-1 * x268 * ((-1 * x235 * ((x898 * x204) + (x895 * x264) + (x895 * x258) + (x204 * ((x895 * x259) + x898 + (x204 * ((x204 * ((-1 * x895 * x262) + x896 + (x895 * x260))) + x897 + (x895 * x263))))))) + (x899 * x266) + (-1 * x234) + (-1 * x233 * x226 * x193))) + x893 + (x894 * x235 * x242 * x162) + (x898 * x241));
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), x271 + (x273 * x271));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), x325 + (x273 * x325));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), x362 + (x273 * x362));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), x389 + (x273 * x389));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), x407 + (x407 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), x420 + (x420 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), x442 + (x442 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), x458 + (x458 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), x470 + (x470 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), x548 + (x548 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), x580 + (x580 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), x612 + (x612 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), x626 + (x626 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), x639 + (x639 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), x651 + (x651 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x750 + (x750 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x805 + (x805 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x843 + (x843 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), x861 + (x861 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), x876 + (x876 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), x888 + (x888 * x273));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.curve)/sizeof(FLT), (-1 * x890 * x273) + (-1 * x890));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.gibmag)/sizeof(FLT), sin(x272));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.gibpha)/sizeof(FLT), x273);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeemag)/sizeof(FLT), (-1 * x891 * x273) + (-1 * x891));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.ogeephase)/sizeof(FLT), (-1 * x892 * x273) + (-1 * x892));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.phase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD0.tilt)/sizeof(FLT), (-1 * x900 * x273) + (-1 * x900));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]

static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_error_model(Hx, dt, _x0, error_model, sensor_pt);
    }
}
// Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]]
static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x2 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x3 = (*_x0).Object.Pose.Rot[0] + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x1 * (*error_model).Object.Pose.AxisAngleRot[1]);
	const FLT x4 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x5 = dt * dt;
	const FLT x6 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x7 = (x6 * x6) * x5;
	const FLT x8 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x9 = x5 * (x8 * x8);
	const FLT x10 = (x4 * x4) * x5;
	const FLT x11 = 1e-10 + x7 + x10 + x9;
	const FLT x12 = sqrt(x11);
	const FLT x13 = 0.5 * x12;
	const FLT x14 = sin(x13);
	const FLT x15 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x16 = (x2 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x1 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x15 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x17 = (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x0 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x15 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2];
	const FLT x18 = (*_x0).Object.Pose.Rot[1] + (x15 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x1 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x19 = (x14 * x14) * (1. / x11);
	const FLT x20 = cos(x13);
	const FLT x21 = (1. / sqrt((x3 * x3) + (x16 * x16) + (x18 * x18) + (x17 * x17))) * (1. / sqrt((x20 * x20) + (x9 * x19) + (x7 * x19) + (x10 * x19)));
	const FLT x22 = dt * x21 * x14 * (1. / x12);
	const FLT x23 = x4 * x22;
	const FLT x24 = x3 * x23;
	const FLT x25 = x20 * x21;
	const FLT x26 = x25 * x17;
	const FLT x27 = x6 * x22;
	const FLT x28 = x27 * x18;
	const FLT x29 = x8 * x22;
	const FLT x30 = x29 * x16;
	const FLT x31 = x28 + (-1 * x30) + x24 + x26;
	const FLT x32 = (x3 * x25) + (-1 * x27 * x16) + (-1 * x29 * x18) + (-1 * x23 * x17);
	const FLT x33 = x3 * x27;
	const FLT x34 = x23 * x18;
	const FLT x35 = x25 * x16;
	const FLT x36 = x29 * x17;
	const FLT x37 = x36 + x35 + x33 + (-1 * x34);
	const FLT x38 = (-1 * x37 * sensor_pt[1]) + (x32 * sensor_pt[0]) + (x31 * sensor_pt[2]);
	const FLT x39 = x27 * x17;
	const FLT x40 = x23 * x16;
	const FLT x41 = x25 * x18;
	const FLT x42 = x3 * x29;
	const FLT x43 = x42 + x41 + (-1 * x39) + x40;
	const FLT x44 = (-1 * x43 * sensor_pt[2]) + (x32 * sensor_pt[1]) + (x37 * sensor_pt[0]);
	const FLT x45 = 1.0/2.0 * dt * fabs(dt);
	const FLT x46 = (*_x0).Object.Pose.Pos[2] + (2 * ((x43 * x44) + (-1 * x31 * x38))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x45 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x47 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x48 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x49 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x50 = (-1 * x49 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x47 * (*_x0).Lighthouse.Rot[3]) + (-1 * x48 * (*_x0).Lighthouse.Rot[2]);
	const FLT x51 = (x48 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x47 * (*_x0).Lighthouse.Rot[0]) + (-1 * x49 * (*_x0).Lighthouse.Rot[2]);
	const FLT x52 = x51 * x51;
	const FLT x53 = x50 * x50;
	const FLT x54 = (-1 * x47 * (*_x0).Lighthouse.Rot[1]) + (x48 * (*_x0).Lighthouse.Rot[0]) + (x49 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2];
	const FLT x55 = x54 * x54;
	const FLT x56 = (*_x0).Lighthouse.Rot[1] + (x49 * (*_x0).Lighthouse.Rot[0]) + (-1 * x48 * (*_x0).Lighthouse.Rot[3]) + (x47 * (*_x0).Lighthouse.Rot[2]);
	const FLT x57 = x56 * x56;
	const FLT x58 = x55 + x52 + x57 + x53;
	const FLT x59 = 1. / x58;
	const FLT x60 = (1. / sqrt((x53 * x59) + (x57 * x59) + (x52 * x59) + (x55 * x59))) * (1. / sqrt(x58));
	const FLT x61 = x60 * x50;
	const FLT x62 = (-1 * x31 * sensor_pt[0]) + (x32 * sensor_pt[2]) + (x43 * sensor_pt[1]);
	const FLT x63 = (*_x0).Object.Pose.Pos[1] + (x45 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x38 * x37) + (-1 * x62 * x43)));
	const FLT x64 = x60 * x56;
	const FLT x65 = (*_x0).Object.Pose.Pos[0] + (x45 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0] + (2 * ((x62 * x31) + (-1 * x44 * x37)));
	const FLT x66 = x60 * x54;
	const FLT x67 = (x65 * x66) + (x61 * x46) + (-1 * x63 * x64);
	const FLT x68 = x60 * x51;
	const FLT x69 = (x63 * x68) + (x61 * x65) + (-1 * x66 * x46);
	const FLT x70 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x71 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x72 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x73 = (x72 * x66) + (x70 * x61) + (-1 * x71 * x64);
	const FLT x74 = (-1 * x70 * x66) + (x71 * x68) + (x72 * x61);
	const FLT x75 = x63 + (2 * ((-1 * x68 * x69) + (x64 * x67))) + (-1 * (x71 + (2 * ((-1 * x74 * x68) + (x73 * x64)))));
	const FLT x76 = x75 * x75;
	const FLT x77 = (x70 * x64) + (x71 * x61) + (-1 * x72 * x68);
	const FLT x78 = (x64 * x46) + (x63 * x61) + (-1 * x68 * x65);
	const FLT x79 = (-1 * (x70 + (2 * ((-1 * x77 * x64) + (x74 * x66))))) + x46 + (2 * ((-1 * x78 * x64) + (x66 * x69)));
	const FLT x80 = x65 + (-1 * (x72 + (2 * ((-1 * x73 * x66) + (x77 * x68))))) + (2 * ((-1 * x67 * x66) + (x78 * x68)));
	const FLT x81 = x80 * x80;
	const FLT x82 = x81 + (x79 * x79);
	const FLT x83 = 1. / x82;
	const FLT x84 = 0.523598775598299 + (*error_model).BSD0.tilt + (*_x0).BSD0.tilt;
	const FLT x85 = tan(x84);
	const FLT x86 = 1. / sqrt(1 + (-1 * x83 * (x85 * x85) * x76));
	const FLT x87 = (-1 * x28) + (-1 * x26) + x30 + (-1 * x24);
	const FLT x88 = 2 * x31;
	const FLT x89 = 1 + (x88 * x87) + (-2 * (x37 * x37));
	const FLT x90 = 2 * x32;
	const FLT x91 = x90 * x31;
	const FLT x92 = 2 * x43;
	const FLT x93 = (x92 * x37) + (-1 * x91);
	const FLT x94 = x90 * x37;
	const FLT x95 = x94 + (-1 * x87 * x92);
	const FLT x96 = (-1 * x64 * x95) + (x89 * x66) + (x61 * x93);
	const FLT x97 = 2 * x66;
	const FLT x98 = (x64 * x93) + (-1 * x89 * x68) + (x61 * x95);
	const FLT x99 = 2 * x68;
	const FLT x100 = x89 + (-1 * x97 * x96) + (x99 * x98);
	const FLT x101 = 2 * x80;
	const FLT x102 = (-1 * x66 * x93) + (x68 * x95) + (x89 * x61);
	const FLT x103 = 2 * x64;
	const FLT x104 = x93 + (x97 * x102) + (-1 * x98 * x103);
	const FLT x105 = 2 * x79;
	const FLT x106 = (x105 * x104) + (x101 * x100);
	const FLT x107 = 1.0/2.0 * x75;
	const FLT x108 = (1. / (x82 * sqrt(x82))) * x85 * x107;
	const FLT x109 = x95 + (-1 * x99 * x102) + (x96 * x103);
	const FLT x110 = (1. / sqrt(x82)) * x85;
	const FLT x111 = (x109 * x110) + (-1 * x108 * x106);
	const FLT x112 = 1. / x80;
	const FLT x113 = (1. / x81) * x79;
	const FLT x114 = x81 * x83;
	const FLT x115 = ((x100 * x113) + (-1 * x104 * x112)) * x114;
	const FLT x116 = (-1 * x115) + (x86 * x111);
	const FLT x117 = (*error_model).BSD0.ogeemag + (*_x0).BSD0.ogeemag;
	const FLT x118 = atan2(-1 * x79, x80);
	const FLT x119 = x75 * x110;
	const FLT x120 = asin(x119) + (-1 * x118) + (-1 * (*error_model).BSD0.ogeephase) + (-1 * (*_x0).BSD0.ogeephase);
	const FLT x121 = x117 * cos(x120);
	const FLT x122 = x82 + x76;
	const FLT x123 = cos(x84);
	const FLT x124 = 1. / x123;
	const FLT x125 = x124 * (1. / sqrt(x122));
	const FLT x126 = asin(x75 * x125);
	const FLT x127 = 8.0108022e-06 * x126;
	const FLT x128 = -8.0108022e-06 + (-1 * x127);
	const FLT x129 = 0.0028679863 + (x128 * x126);
	const FLT x130 = 5.3685255e-06 + (x126 * x129);
	const FLT x131 = 0.0076069798 + (x126 * x130);
	const FLT x132 = x126 * x131;
	const FLT x133 = -8.0108022e-06 + (-1.60216044e-05 * x126);
	const FLT x134 = x129 + (x126 * x133);
	const FLT x135 = x130 + (x126 * x134);
	const FLT x136 = x131 + (x126 * x135);
	const FLT x137 = (x126 * x136) + x132;
	const FLT x138 = (*_x0).BSD0.curve + (-1 * x117 * sin(x120)) + (*error_model).BSD0.curve;
	const FLT x139 = sin(x84);
	const FLT x140 = x138 * x139;
	const FLT x141 = x123 + (-1 * x137 * x140);
	const FLT x142 = 1. / x141;
	const FLT x143 = x126 * x126;
	const FLT x144 = x131 * x142 * x143;
	const FLT x145 = x121 * x144;
	const FLT x146 = 1. / sqrt(1 + (-1 * x76 * (1. / (x123 * x123)) * (1. / x122)));
	const FLT x147 = 2 * x75;
	const FLT x148 = x107 * x124 * (1. / (x122 * sqrt(x122)));
	const FLT x149 = (x109 * x125) + (-1 * x148 * (x106 + (x109 * x147)));
	const FLT x150 = x146 * x149;
	const FLT x151 = x128 * x150;
	const FLT x152 = (x126 * (x151 + (-1 * x127 * x150))) + (x129 * x150);
	const FLT x153 = (x130 * x150) + (x126 * x152);
	const FLT x154 = x138 * x143;
	const FLT x155 = x142 * x154;
	const FLT x156 = 2 * x132 * x138 * x142;
	const FLT x157 = 2.40324066e-05 * x126;
	const FLT x158 = x131 * x146;
	const FLT x159 = x137 * x139;
	const FLT x160 = x121 * x159;
	const FLT x161 = x131 * (1. / (x141 * x141)) * x154;
	const FLT x162 = x119 + (x131 * x155);
	const FLT x163 = 1. / sqrt(1 + (-1 * (x162 * x162)));
	const FLT x164 = x115 + (-1 * x163 * (x111 + (-1 * x161 * ((x116 * x160) + (-1 * x140 * ((x149 * x158) + (x136 * x150) + (x126 * x153) + (x126 * ((x135 * x150) + x153 + (x126 * (x152 + (x126 * ((-1 * x150 * x157) + x151 + (x133 * x150))) + (x134 * x150))))))))) + (x150 * x156) + (-1 * x116 * x145) + (x153 * x155)));
	const FLT x165 = ((*error_model).BSD0.gibmag + (*_x0).BSD0.gibmag) * cos(x118 + (*error_model).BSD0.gibpha + (-1 * asin(x162)) + (*_x0).BSD0.gibpha);
	const FLT x166 = (-1 * x33) + (-1 * x36) + x34 + (-1 * x35);
	const FLT x167 = 2 * x37;
	const FLT x168 = 1 + (x167 * x166) + (-2 * (x43 * x43));
	const FLT x169 = (x92 * x31) + (-1 * x94);
	const FLT x170 = x90 * x43;
	const FLT x171 = x170 + (-1 * x88 * x166);
	const FLT x172 = (-1 * x66 * x171) + (x68 * x168) + (x61 * x169);
	const FLT x173 = (-1 * x64 * x168) + (x66 * x169) + (x61 * x171);
	const FLT x174 = x168 + (-1 * x99 * x172) + (x103 * x173);
	const FLT x175 = (-1 * x68 * x169) + (x64 * x171) + (x61 * x168);
	const FLT x176 = x169 + (-1 * x97 * x173) + (x99 * x175);
	const FLT x177 = x171 + (x97 * x172) + (-1 * x103 * x175);
	const FLT x178 = (x105 * x177) + (x101 * x176);
	const FLT x179 = (x125 * x174) + (-1 * x148 * (x178 + (x174 * x147)));
	const FLT x180 = x179 * x146;
	const FLT x181 = x128 * x180;
	const FLT x182 = (x126 * (x181 + (-1 * x127 * x180))) + (x129 * x180);
	const FLT x183 = (x180 * x130) + (x126 * x182);
	const FLT x184 = (x110 * x174) + (-1 * x108 * x178);
	const FLT x185 = ((x113 * x176) + (-1 * x112 * x177)) * x114;
	const FLT x186 = x121 * ((-1 * x185) + (x86 * x184));
	const FLT x187 = x185 + (-1 * x163 * (x184 + (-1 * x186 * x144) + (-1 * x161 * ((x186 * x159) + (-1 * x140 * ((x126 * x183) + (x179 * x158) + (x180 * x136) + (x126 * (x183 + (x180 * x135) + (x126 * (x182 + (x126 * (x181 + (-1 * x180 * x157) + (x180 * x133))) + (x180 * x134))))))))) + (x183 * x155) + (x180 * x156)));
	const FLT x188 = (x88 * x37) + (-1 * x170);
	const FLT x189 = (-1 * x41) + (-1 * x42) + (-1 * x40) + x39;
	const FLT x190 = x91 + (-1 * x167 * x189);
	const FLT x191 = 1 + (x92 * x189) + (-2 * (x31 * x31));
	const FLT x192 = (-1 * x66 * x191) + (x68 * x188) + (x61 * x190);
	const FLT x193 = (-1 * x64 * x188) + (x66 * x190) + (x61 * x191);
	const FLT x194 = x188 + (-1 * x99 * x192) + (x103 * x193);
	const FLT x195 = (-1 * x68 * x190) + (x64 * x191) + (x61 * x188);
	const FLT x196 = x190 + (-1 * x97 * x193) + (x99 * x195);
	const FLT x197 = x191 + (x97 * x192) + (-1 * x103 * x195);
	const FLT x198 = (x105 * x197) + (x101 * x196);
	const FLT x199 = (x125 * x194) + (-1 * x148 * (x198 + (x194 * x147)));
	const FLT x200 = x199 * x146;
	const FLT x201 = x200 * x128;
	const FLT x202 = (x126 * (x201 + (-1 * x200 * x127))) + (x200 * x129);
	const FLT x203 = (x200 * x130) + (x202 * x126);
	const FLT x204 = (x110 * x194) + (-1 * x108 * x198);
	const FLT x205 = ((x113 * x196) + (-1 * x112 * x197)) * x114;
	const FLT x206 = (-1 * x205) + (x86 * x204);
	const FLT x207 = x205 + (-1 * x163 * ((-1 * x206 * x145) + (x203 * x155) + x204 + (-1 * x161 * ((x206 * x160) + (-1 * x140 * ((x199 * x158) + (x200 * x136) + (x203 * x126) + (x126 * ((x200 * x135) + x203 + (x126 * (x202 + (x126 * ((-1 * x200 * x157) + (x200 * x133) + x201)) + (x200 * x134))))))))) + (x200 * x156)));
	cnMatrixOptionalSet(Hx, 0, 0, x164 + (x165 * x164));
	cnMatrixOptionalSet(Hx, 0, 1, x187 + (x165 * x187));
	cnMatrixOptionalSet(Hx, 0, 2, x207 + (x207 * x165));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_x_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]]

static inline void SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_x_gen2(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_x_gen2_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt);
    }
}
static inline FLT SurviveJointKalmanErrorModel_LightMeas_y_gen2(const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x3 = (x1 * (*_x0).Lighthouse.Rot[0]) + (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (x0 * (*_x0).Lighthouse.Rot[3]) + (*_x0).Lighthouse.Rot[2];
	const FLT x4 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x5 = (x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x2 * (*_x0).Lighthouse.Rot[0]) + (-1 * x0 * (*_x0).Lighthouse.Rot[2]);
	const FLT x6 = x5 * x5;
	const FLT x7 = (*_x0).Lighthouse.Rot[0] + (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (-1 * x2 * (*_x0).Lighthouse.Rot[3]) + (-1 * x1 * (*_x0).Lighthouse.Rot[2]);
	const FLT x8 = x7 * x7;
	const FLT x9 = x3 * x3;
	const FLT x10 = (*_x0).Lighthouse.Rot[1] + (-1 * x1 * (*_x0).Lighthouse.Rot[3]) + (x0 * (*_x0).Lighthouse.Rot[0]) + (x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x11 = x10 * x10;
	const FLT x12 = x11 + x9 + x6 + x8;
	const FLT x13 = 1. / x12;
	const FLT x14 = (1. / sqrt(x12)) * (1. / sqrt((x13 * x11) + (x8 * x13) + (x6 * x13) + (x9 * x13)));
	const FLT x15 = x3 * x14;
	const FLT x16 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x17 = x7 * x14;
	const FLT x18 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x19 = x5 * x14;
	const FLT x20 = x14 * ((-1 * x4 * x15) + (x19 * x18) + (x17 * x16));
	const FLT x21 = x14 * x10;
	const FLT x22 = (x4 * x21) + (x18 * x17) + (-1 * x19 * x16);
	const FLT x23 = 1.0/2.0 * dt * fabs(dt);
	const FLT x24 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x25 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x26 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x27 = (-1 * x26 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x24 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x25 * (*_x0).Object.Pose.Rot[2]);
	const FLT x28 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x29 = dt * dt;
	const FLT x30 = (x28 * x28) * x29;
	const FLT x31 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x32 = (x31 * x31) * x29;
	const FLT x33 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x34 = (x33 * x33) * x29;
	const FLT x35 = 1e-10 + x34 + x30 + x32;
	const FLT x36 = sqrt(x35);
	const FLT x37 = 0.5 * x36;
	const FLT x38 = sin(x37);
	const FLT x39 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x40 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x41 = (-1 * x40 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x25 * (*_x0).Object.Pose.Rot[1]) + (x39 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x42 = (-1 * x26 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x25 * (*_x0).Object.Pose.Rot[0]) + (x24 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2];
	const FLT x43 = (*_x0).Object.Pose.Rot[1] + (x39 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x25 * (*_x0).Object.Pose.Rot[3]) + (x40 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x44 = (1. / x35) * (x38 * x38);
	const FLT x45 = cos(x37);
	const FLT x46 = (1. / sqrt((x43 * x43) + (x27 * x27) + (x41 * x41) + (x42 * x42))) * (1. / sqrt((x44 * x32) + (x45 * x45) + (x44 * x30) + (x44 * x34)));
	const FLT x47 = dt * x46 * (1. / x36) * x38;
	const FLT x48 = x47 * x28;
	const FLT x49 = x43 * x47;
	const FLT x50 = x45 * x46;
	const FLT x51 = x47 * x31;
	const FLT x52 = (x51 * x42) + (x50 * x41) + (x48 * x27) + (-1 * x49 * x33);
	const FLT x53 = x47 * x33;
	const FLT x54 = (-1 * x49 * x31) + (-1 * x41 * x48) + (x50 * x27) + (-1 * x53 * x42);
	const FLT x55 = (x51 * x27) + (x50 * x43) + (-1 * x42 * x48) + (x53 * x41);
	const FLT x56 = (-1 * x55 * sensor_pt[2]) + (x54 * sensor_pt[1]) + (x52 * sensor_pt[0]);
	const FLT x57 = (-1 * x51 * x41) + (x53 * x27) + (x49 * x28) + (x50 * x42);
	const FLT x58 = (-1 * x57 * sensor_pt[0]) + (x54 * sensor_pt[2]) + (x55 * sensor_pt[1]);
	const FLT x59 = (*_x0).Object.Pose.Pos[0] + (2 * ((x58 * x57) + (-1 * x52 * x56))) + (x23 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0]));
	const FLT x60 = (-1 * x52 * sensor_pt[1]) + (x54 * sensor_pt[0]) + (x57 * sensor_pt[2]);
	const FLT x61 = (*_x0).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (2 * ((x56 * x55) + (-1 * x60 * x57))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (x23 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x62 = (*_x0).Object.Pose.Pos[1] + (x23 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x60 * x52) + (-1 * x58 * x55)));
	const FLT x63 = (x62 * x19) + (x59 * x17) + (-1 * x61 * x15);
	const FLT x64 = (x61 * x21) + (x62 * x17) + (-1 * x59 * x19);
	const FLT x65 = x61 + (-1 * (x4 + (2 * ((-1 * x22 * x21) + (x3 * x20))))) + (2 * ((-1 * x64 * x21) + (x63 * x15)));
	const FLT x66 = (x15 * x16) + (x4 * x17) + (-1 * x21 * x18);
	const FLT x67 = (x59 * x15) + (x61 * x17) + (-1 * x62 * x21);
	const FLT x68 = x59 + (-1 * (x16 + (2 * ((-1 * x66 * x15) + (x22 * x19))))) + (2 * ((-1 * x67 * x15) + (x64 * x19)));
	const FLT x69 = atan2(-1 * x65, x68);
	const FLT x70 = (2 * ((-1 * x63 * x19) + (x67 * x21))) + x62 + (-1 * (x18 + (2 * ((-1 * x5 * x20) + (x66 * x21)))));
	const FLT x71 = (x68 * x68) + (x65 * x65);
	const FLT x72 = 0.523598775598299 + (-1 * (*error_model).BSD1.tilt) + (-1 * (*_x0).BSD1.tilt);
	const FLT x73 = cos(x72);
	const FLT x74 = asin(x70 * (1. / x73) * (1. / sqrt(x71 + (x70 * x70))));
	const FLT x75 = 0.0028679863 + (x74 * (-8.0108022e-06 + (-8.0108022e-06 * x74)));
	const FLT x76 = 5.3685255e-06 + (x75 * x74);
	const FLT x77 = 0.0076069798 + (x74 * x76);
	const FLT x78 = -1 * (1. / sqrt(x71)) * x70 * tan(x72);
	const FLT x79 = (*_x0).BSD1.curve + (((*error_model).BSD1.ogeemag + (*_x0).BSD1.ogeemag) * sin((-1 * asin(x78)) + (*_x0).BSD1.ogeephase + (*error_model).BSD1.ogeephase + x69)) + (*error_model).BSD1.curve;
	const FLT x80 = (-1 * asin(x78 + (x79 * (x74 * x74) * x77 * (1. / (x73 + (x79 * sin(x72) * ((x74 * (x77 + (x74 * (x76 + (x74 * (x75 + (x74 * (-8.0108022e-06 + (-1.60216044e-05 * x74))))))))) + (x74 * x77)))))))) + x69;
	return -1.5707963267949 + x80 + (-1 * ((*error_model).BSD1.phase + (*_x0).BSD1.phase)) + (((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag) * sin(x80 + (*error_model).BSD1.gibpha + (*_x0).BSD1.gibpha));
}

// Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_x0(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*_x0).Lighthouse.Rot[3];
	const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x3 = (-1 * x2 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (x0 * (*error_model).Lighthouse.AxisAngleRot[0]) + (*_x0).Lighthouse.Rot[2];
	const FLT x4 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x5 = 0.5 * (*_x0).Lighthouse.Rot[2];
	const FLT x6 = (x1 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[3] + (x2 * (*_x0).Lighthouse.Rot[0]) + (-1 * x5 * (*error_model).Lighthouse.AxisAngleRot[0]);
	const FLT x7 = x6 * x6;
	const FLT x8 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x9 = (-1 * x0 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x8 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x5 * (*error_model).Lighthouse.AxisAngleRot[1]);
	const FLT x10 = x9 * x9;
	const FLT x11 = x3 * x3;
	const FLT x12 = (*_x0).Lighthouse.Rot[1] + (x8 * (*_x0).Lighthouse.Rot[0]) + (-1 * x0 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x13 = x12 * x12;
	const FLT x14 = x13 + x11 + x7 + x10;
	const FLT x15 = 1. / x14;
	const FLT x16 = x7 * x15;
	const FLT x17 = x15 * x11;
	const FLT x18 = x15 * x13;
	const FLT x19 = x18 + (x15 * x10) + x16 + x17;
	const FLT x20 = 1. / sqrt(x19);
	const FLT x21 = 1. / sqrt(x14);
	const FLT x22 = x20 * x21;
	const FLT x23 = x4 * x22;
	const FLT x24 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x25 = x24 * x22;
	const FLT x26 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x27 = x22 * x26;
	const FLT x28 = (x6 * x27) + (-1 * x3 * x23) + (x9 * x25);
	const FLT x29 = x3 * x22;
	const FLT x30 = (x23 * x12) + (x9 * x27) + (-1 * x6 * x25);
	const FLT x31 = x22 * x12;
	const FLT x32 = dt * fabs(dt);
	const FLT x33 = 1.0/2.0 * x32;
	const FLT x34 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x35 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x36 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x37 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x38 = (-1 * x37 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[0] + (-1 * x35 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x36 * (*error_model).Object.Pose.AxisAngleRot[1]);
	const FLT x39 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x40 = (-1 * x36 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x37 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x39 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x41 = (-1 * x37 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x39 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x35 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[2];
	const FLT x42 = (*_x0).Object.Pose.Rot[1] + (x39 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x35 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x36 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x43 = (x42 * x42) + (x40 * x40) + (x38 * x38) + (x41 * x41);
	const FLT x44 = 1. / sqrt(x43);
	const FLT x45 = dt * dt;
	const FLT x46 = x34 * x34;
	const FLT x47 = x45 * x46;
	const FLT x48 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x49 = x48 * x48;
	const FLT x50 = x45 * x49;
	const FLT x51 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x52 = x51 * x51;
	const FLT x53 = x52 * x45;
	const FLT x54 = 1e-10 + x47 + x53 + x50;
	const FLT x55 = sqrt(x54);
	const FLT x56 = 0.5 * x55;
	const FLT x57 = sin(x56);
	const FLT x58 = x57 * x57;
	const FLT x59 = 1. / x54;
	const FLT x60 = x58 * x59;
	const FLT x61 = cos(x56);
	const FLT x62 = (x60 * x50) + (x61 * x61) + (x60 * x47) + (x60 * x53);
	const FLT x63 = 1. / sqrt(x62);
	const FLT x64 = x57 * (1. / x55);
	const FLT x65 = x63 * x64;
	const FLT x66 = dt * x65;
	const FLT x67 = x66 * x44;
	const FLT x68 = x67 * x38;
	const FLT x69 = x67 * x42;
	const FLT x70 = x63 * x61;
	const FLT x71 = x70 * x44;
	const FLT x72 = x71 * x40;
	const FLT x73 = x41 * x44;
	const FLT x74 = x73 * x66;
	const FLT x75 = (x74 * x48) + x72 + (x68 * x34) + (-1 * x69 * x51);
	const FLT x76 = x40 * x44;
	const FLT x77 = x76 * x66;
	const FLT x78 = x71 * x38;
	const FLT x79 = x78 + (-1 * x69 * x48) + (-1 * x77 * x34) + (-1 * x74 * x51);
	const FLT x80 = x71 * x42;
	const FLT x81 = (x68 * x48) + x80 + (-1 * x74 * x34) + (x77 * x51);
	const FLT x82 = (-1 * x81 * sensor_pt[2]) + (x79 * sensor_pt[1]) + (x75 * sensor_pt[0]);
	const FLT x83 = x71 * x41;
	const FLT x84 = (-1 * x77 * x48) + (x69 * x34) + (x68 * x51) + x83;
	const FLT x85 = (-1 * x84 * sensor_pt[0]) + (x79 * sensor_pt[2]) + (x81 * sensor_pt[1]);
	const FLT x86 = (*_x0).Object.Pose.Pos[0] + (x33 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + (2 * ((x84 * x85) + (-1 * x82 * x75)));
	const FLT x87 = x86 * x22;
	const FLT x88 = (-1 * x75 * sensor_pt[1]) + (x79 * sensor_pt[0]) + (x84 * sensor_pt[2]);
	const FLT x89 = (*_x0).Object.Pose.Pos[2] + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (2 * ((x81 * x82) + (-1 * x88 * x84))) + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x33 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x90 = x89 * x22;
	const FLT x91 = (*_x0).Object.Pose.Pos[1] + (x33 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (2 * ((x88 * x75) + (-1 * x81 * x85)));
	const FLT x92 = x91 * x22;
	const FLT x93 = (x6 * x92) + (x9 * x87) + (-1 * x3 * x90);
	const FLT x94 = (x9 * x92) + (x90 * x12) + (-1 * x6 * x87);
	const FLT x95 = x89 + (-1 * (x4 + (2 * ((-1 * x30 * x31) + (x28 * x29))))) + (2 * ((-1 * x94 * x31) + (x93 * x29)));
	const FLT x96 = x6 * x22;
	const FLT x97 = (x9 * x23) + (x3 * x25) + (-1 * x27 * x12);
	const FLT x98 = (x3 * x87) + (x9 * x90) + (-1 * x92 * x12);
	const FLT x99 = x98 * x22;
	const FLT x100 = x86 + (-1 * (x24 + (2 * ((-1 * x97 * x29) + (x96 * x30))))) + (2 * ((-1 * x3 * x99) + (x96 * x94)));
	const FLT x101 = atan2(-1 * x95, x100);
	const FLT x102 = (2 * ((-1 * x93 * x96) + (x99 * x12))) + x91 + (-1 * (x26 + (2 * ((-1 * x96 * x28) + (x97 * x31)))));
	const FLT x103 = x100 * x100;
	const FLT x104 = x103 + (x95 * x95);
	const FLT x105 = 1. / sqrt(x104);
	const FLT x106 = 0.523598775598299 + (-1 * (*error_model).BSD1.tilt) + (-1 * (*_x0).BSD1.tilt);
	const FLT x107 = tan(x106);
	const FLT x108 = x105 * x107;
	const FLT x109 = -1 * x102 * x108;
	const FLT x110 = asin(x109) + (-1 * (*_x0).BSD1.ogeephase) + (-1 * (*error_model).BSD1.ogeephase) + (-1 * x101);
	const FLT x111 = sin(x110);
	const FLT x112 = (*error_model).BSD1.ogeemag + (*_x0).BSD1.ogeemag;
	const FLT x113 = (*_x0).BSD1.curve + (-1 * x111 * x112) + (*error_model).BSD1.curve;
	const FLT x114 = x102 * x102;
	const FLT x115 = x104 + x114;
	const FLT x116 = 1. / sqrt(x115);
	const FLT x117 = cos(x106);
	const FLT x118 = 1. / x117;
	const FLT x119 = x118 * x116;
	const FLT x120 = asin(x102 * x119);
	const FLT x121 = 8.0108022e-06 * x120;
	const FLT x122 = -8.0108022e-06 + (-1 * x121);
	const FLT x123 = 0.0028679863 + (x120 * x122);
	const FLT x124 = 5.3685255e-06 + (x120 * x123);
	const FLT x125 = 0.0076069798 + (x120 * x124);
	const FLT x126 = x120 * x120;
	const FLT x127 = sin(x106);
	const FLT x128 = x120 * x125;
	const FLT x129 = -8.0108022e-06 + (-1.60216044e-05 * x120);
	const FLT x130 = x123 + (x120 * x129);
	const FLT x131 = x124 + (x120 * x130);
	const FLT x132 = x125 + (x120 * x131);
	const FLT x133 = (x120 * x132) + x128;
	const FLT x134 = x113 * x133;
	const FLT x135 = x127 * x134;
	const FLT x136 = x117 + x135;
	const FLT x137 = 1. / x136;
	const FLT x138 = x126 * x137;
	const FLT x139 = x125 * x138;
	const FLT x140 = x109 + (x113 * x139);
	const FLT x141 = 1. / sqrt(1 + (-1 * (x140 * x140)));
	const FLT x142 = 1. / x19;
	const FLT x143 = 2 * x9;
	const FLT x144 = x15 * x143;
	const FLT x145 = x6 * x142 * x144;
	const FLT x146 = 2 * x12;
	const FLT x147 = x15 * x146;
	const FLT x148 = x3 * x142;
	const FLT x149 = x148 * x147;
	const FLT x150 = -1 * x149;
	const FLT x151 = x150 + x145;
	const FLT x152 = 2 * x102;
	const FLT x153 = 2 * x142;
	const FLT x154 = x16 * x153;
	const FLT x155 = x17 * x153;
	const FLT x156 = -1 + x155;
	const FLT x157 = x156 + x154;
	const FLT x158 = 2 * x100;
	const FLT x159 = 2 * x6;
	const FLT x160 = x15 * x159;
	const FLT x161 = x12 * x142;
	const FLT x162 = x161 * x160;
	const FLT x163 = -1 * x162;
	const FLT x164 = x144 * x148;
	const FLT x165 = -1 * x164;
	const FLT x166 = x165 + x163;
	const FLT x167 = 2 * x95;
	const FLT x168 = (x167 * x166) + (x157 * x158);
	const FLT x169 = 1.0/2.0 * x102;
	const FLT x170 = x118 * (1. / (x115 * sqrt(x115))) * x169;
	const FLT x171 = (x119 * x151) + (-1 * x170 * (x168 + (x151 * x152)));
	const FLT x172 = 1. / (x117 * x117);
	const FLT x173 = 1. / sqrt(1 + (-1 * x114 * (1. / x115) * x172));
	const FLT x174 = x121 * x173;
	const FLT x175 = x122 * x173;
	const FLT x176 = x171 * x175;
	const FLT x177 = x123 * x173;
	const FLT x178 = (x171 * x177) + (x120 * (x176 + (-1 * x171 * x174)));
	const FLT x179 = x124 * x173;
	const FLT x180 = (x171 * x179) + (x120 * x178);
	const FLT x181 = x113 * x138;
	const FLT x182 = 1. / x104;
	const FLT x183 = x107 * x107;
	const FLT x184 = 1. / sqrt(1 + (-1 * x114 * x183 * x182));
	const FLT x185 = (1. / (x104 * sqrt(x104))) * x107 * x169;
	const FLT x186 = (-1 * x108 * x151) + (x168 * x185);
	const FLT x187 = 1. / x100;
	const FLT x188 = x95 * (1. / x103);
	const FLT x189 = x103 * x182;
	const FLT x190 = ((x188 * x157) + (-1 * x166 * x187)) * x189;
	const FLT x191 = (-1 * x190) + (x186 * x184);
	const FLT x192 = x112 * cos(x110);
	const FLT x193 = x127 * x133;
	const FLT x194 = x192 * x193;
	const FLT x195 = x173 * x131;
	const FLT x196 = x129 * x173;
	const FLT x197 = 2.40324066e-05 * x120;
	const FLT x198 = x173 * x197;
	const FLT x199 = x173 * x130;
	const FLT x200 = x125 * x173;
	const FLT x201 = x173 * x132;
	const FLT x202 = x113 * x127;
	const FLT x203 = x126 * x125 * (1. / (x136 * x136));
	const FLT x204 = x203 * x113;
	const FLT x205 = x128 * x173 * x137;
	const FLT x206 = 2 * x113;
	const FLT x207 = x205 * x206;
	const FLT x208 = x192 * x139;
	const FLT x209 = x190 + (-1 * x141 * ((-1 * x208 * x191) + (x207 * x171) + (x181 * x180) + x186 + (-1 * x204 * ((x202 * ((x120 * x180) + (x201 * x171) + (x120 * ((x171 * x195) + x180 + (x120 * ((x120 * ((-1 * x171 * x198) + x176 + (x171 * x196))) + x178 + (x171 * x199))))) + (x200 * x171))) + (-1 * x191 * x194)))));
	const FLT x210 = (-1 * asin(x140)) + (*error_model).BSD1.gibpha + x101 + (*_x0).BSD1.gibpha;
	const FLT x211 = cos(x210) * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag);
	const FLT x212 = x18 * x153;
	const FLT x213 = -1 + x212 + x154;
	const FLT x214 = -1 * x145;
	const FLT x215 = x214 + x150;
	const FLT x216 = x161 * x144;
	const FLT x217 = x160 * x148;
	const FLT x218 = -1 * x217;
	const FLT x219 = x218 + x216;
	const FLT x220 = (x219 * x167) + (x215 * x158);
	const FLT x221 = (x213 * x119) + (-1 * x170 * (x220 + (x213 * x152)));
	const FLT x222 = (-1 * x213 * x108) + (x220 * x185);
	const FLT x223 = ((x215 * x188) + (-1 * x219 * x187)) * x189;
	const FLT x224 = (-1 * x223) + (x222 * x184);
	const FLT x225 = x221 * x175;
	const FLT x226 = (x221 * x177) + (x120 * (x225 + (-1 * x221 * x174)));
	const FLT x227 = (x221 * x179) + (x226 * x120);
	const FLT x228 = x223 + (-1 * x141 * (x222 + (x227 * x181) + (-1 * x208 * x224) + (x207 * x221) + (-1 * x204 * ((x202 * ((x227 * x120) + (x200 * x221) + (x201 * x221) + (x120 * (x227 + (x221 * x195) + (x120 * (x226 + (x120 * ((-1 * x221 * x198) + x225 + (x221 * x196))) + (x221 * x199))))))) + (-1 * x224 * x194)))));
	const FLT x229 = x163 + x164;
	const FLT x230 = x156 + x212;
	const FLT x231 = (x230 * x167) + (x229 * x158);
	const FLT x232 = -1 * x216;
	const FLT x233 = x232 + x218;
	const FLT x234 = (-1 * x233 * x108) + (x231 * x185);
	const FLT x235 = ((x229 * x188) + (-1 * x230 * x187)) * x189;
	const FLT x236 = (-1 * x235) + (x234 * x184);
	const FLT x237 = (x233 * x119) + (-1 * x170 * (x231 + (x233 * x152)));
	const FLT x238 = x237 * x175;
	const FLT x239 = (x237 * x177) + (x120 * (x238 + (-1 * x237 * x174)));
	const FLT x240 = (x237 * x179) + (x239 * x120);
	const FLT x241 = x235 + (-1 * x141 * ((x237 * x207) + (-1 * x236 * x208) + (-1 * x204 * ((x202 * ((x237 * x201) + (x240 * x120) + (x237 * x200) + (x120 * (x240 + (x237 * x195) + (x120 * (x239 + (x120 * ((-1 * x237 * x198) + x238 + (x237 * x196))) + (x237 * x199))))))) + (-1 * x236 * x194))) + x234 + (x240 * x181)));
	const FLT x242 = 1.0 * x3;
	const FLT x243 = x242 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x244 = 1.0 * x12;
	const FLT x245 = x244 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x246 = 1.0 * x6;
	const FLT x247 = x246 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x248 = x143 + x247 + x243 + x245;
	const FLT x249 = 1. / (x14 * x14);
	const FLT x250 = x10 * x249;
	const FLT x251 = x7 * x249;
	const FLT x252 = x13 * x249;
	const FLT x253 = x11 * x249;
	const FLT x254 = x144 + (x15 * x243) + (-1 * x253 * x248) + (x15 * x245) + (-1 * x252 * x248) + (-1 * x251 * x248) + (-1 * x250 * x248) + (x15 * x247);
	const FLT x255 = x21 * (1. / (x19 * sqrt(x19)));
	const FLT x256 = x254 * x255;
	const FLT x257 = x97 * x12;
	const FLT x258 = 1.0 * x22;
	const FLT x259 = x97 * x258;
	const FLT x260 = x259 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x261 = x6 * x28;
	const FLT x262 = x20 * (1. / (x14 * sqrt(x14)));
	const FLT x263 = x262 * x248;
	const FLT x264 = x3 * x86;
	const FLT x265 = 1.0/2.0 * x263;
	const FLT x266 = 1.0/2.0 * x256;
	const FLT x267 = x86 * x266;
	const FLT x268 = 1.0/2.0 * x91;
	const FLT x269 = x268 * x263;
	const FLT x270 = x9 * x266;
	const FLT x271 = x91 * x266;
	const FLT x272 = 1.0/2.0 * x89;
	const FLT x273 = x9 * x272;
	const FLT x274 = x1 * x87;
	const FLT x275 = -1 * x8 * x92;
	const FLT x276 = x275 + x90;
	const FLT x277 = x274 + (-1 * x273 * x263) + (-1 * x3 * x267) + x276 + (-1 * x264 * x265) + (x12 * x269) + (-1 * x89 * x270) + (x12 * x271);
	const FLT x278 = x22 * x146;
	const FLT x279 = x98 * x12;
	const FLT x280 = x6 * x93;
	const FLT x281 = 1.0 * x99;
	const FLT x282 = x281 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x283 = x6 * x265;
	const FLT x284 = x2 * x27;
	const FLT x285 = x9 * x265;
	const FLT x286 = x4 * x3;
	const FLT x287 = x6 * x26;
	const FLT x288 = -1 * x1 * x23;
	const FLT x289 = (-1 * x24 * x270) + x288 + x284 + (x265 * x286) + (x266 * x286) + x25 + (-1 * x26 * x283) + (-1 * x24 * x285) + (-1 * x266 * x287);
	const FLT x290 = x22 * x159;
	const FLT x291 = x6 * x268;
	const FLT x292 = x2 * x92;
	const FLT x293 = x3 * x272;
	const FLT x294 = x89 * x266;
	const FLT x295 = -1 * x1 * x90;
	const FLT x296 = x87 + x295;
	const FLT x297 = x296 + (x3 * x294) + (x293 * x263) + (-1 * x86 * x270) + (-1 * x6 * x271) + x292 + (-1 * x291 * x263) + (-1 * x86 * x285);
	const FLT x298 = x3 * x24;
	const FLT x299 = x26 * x12;
	const FLT x300 = x4 * x9;
	const FLT x301 = 1.0/2.0 * x300;
	const FLT x302 = x1 * x25;
	const FLT x303 = -1 * x8 * x27;
	const FLT x304 = x303 + x23;
	const FLT x305 = (-1 * x298 * x265) + (x299 * x265) + (x299 * x266) + x302 + (-1 * x266 * x300) + x304 + (-1 * x298 * x266) + (-1 * x263 * x301);
	const FLT x306 = x93 * x258;
	const FLT x307 = x28 * x258;
	const FLT x308 = (x307 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x306 * (*error_model).Lighthouse.AxisAngleRot[2]);
	const FLT x309 = x308 + (-1 * x297 * x290) + (-1 * x279 * x263) + (-1 * x263 * x261) + (-1 * x260) + (x278 * x277) + (x256 * x257) + (-1 * x278 * x305) + (x263 * x257) + (-1 * x261 * x256) + (x290 * x289) + (x280 * x256) + x282 + (-1 * x279 * x256) + (x263 * x280);
	const FLT x310 = x8 * x90;
	const FLT x311 = -1 * x2 * x87;
	const FLT x312 = x12 * x272;
	const FLT x313 = x310 + (-1 * x263 * x312) + (-1 * x12 * x294) + (x6 * x267) + (-1 * x9 * x269) + x92 + (x86 * x283) + (-1 * x91 * x270) + x311;
	const FLT x314 = x94 * x258;
	const FLT x315 = x314 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x316 = x94 * x263;
	const FLT x317 = x6 * x30;
	const FLT x318 = x94 * x255;
	const FLT x319 = x254 * x318;
	const FLT x320 = x30 * x258;
	const FLT x321 = x320 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x322 = x4 * x12;
	const FLT x323 = x9 * x26;
	const FLT x324 = x6 * x24;
	const FLT x325 = x8 * x23;
	const FLT x326 = -1 * x2 * x25;
	const FLT x327 = x326 + x27;
	const FLT x328 = (-1 * x26 * x270) + x325 + (-1 * x266 * x322) + (-1 * x265 * x322) + x327 + (x24 * x283) + (-1 * x265 * x323) + (x266 * x324);
	const FLT x329 = x3 * x98;
	const FLT x330 = 2 * x3;
	const FLT x331 = x22 * x330;
	const FLT x332 = x3 * x97;
	const FLT x333 = x258 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x334 = (-1 * x281 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x97 * x333);
	const FLT x335 = x334 + (x256 * x329) + (-1 * x277 * x331) + (-1 * x256 * x332) + (-1 * x6 * x316) + (x305 * x331) + (-1 * x263 * x332) + (x290 * x313) + (x263 * x317) + (x256 * x317) + (-1 * x290 * x328) + x315 + (-1 * x321) + (-1 * x6 * x319) + (x263 * x329);
	const FLT x336 = x3 * x93;
	const FLT x337 = x30 * x12;
	const FLT x338 = x3 * x28;
	const FLT x339 = x93 * x333;
	const FLT x340 = x28 * x333;
	const FLT x341 = (x320 * (*error_model).Lighthouse.AxisAngleRot[0]) + (-1 * x314 * (*error_model).Lighthouse.AxisAngleRot[0]);
	const FLT x342 = (-1 * x289 * x331) + x341 + (-1 * x256 * x336) + (-1 * x256 * x337) + (x263 * x338) + (-1 * x263 * x337) + (x12 * x316) + (x278 * x328) + (-1 * x263 * x336) + x339 + (x297 * x331) + (-1 * x340) + (x256 * x338) + (-1 * x278 * x313) + (x12 * x319);
	const FLT x343 = (x342 * x167) + (x335 * x158);
	const FLT x344 = (x309 * x119) + (-1 * x170 * (x343 + (x309 * x152)));
	const FLT x345 = (-1 * x309 * x108) + (x343 * x185);
	const FLT x346 = ((x335 * x188) + (-1 * x342 * x187)) * x189;
	const FLT x347 = (-1 * x346) + (x345 * x184);
	const FLT x348 = x344 * x175;
	const FLT x349 = (x344 * x177) + (x120 * (x348 + (-1 * x344 * x174)));
	const FLT x350 = (x344 * x179) + (x349 * x120);
	const FLT x351 = x346 + (-1 * x141 * (x345 + (-1 * x208 * x347) + (x207 * x344) + (x350 * x181) + (-1 * x204 * ((x202 * ((x350 * x120) + (x200 * x344) + (x201 * x344) + (x120 * (x350 + (x344 * x195) + (x120 * (x349 + (x120 * ((-1 * x344 * x198) + x348 + (x344 * x196))) + (x344 * x199))))))) + (-1 * x347 * x194)))));
	const FLT x352 = x246 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x353 = x242 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x354 = 1.0 * x9;
	const FLT x355 = x354 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x356 = (-1 * x353) + (-1 * x355) + x352 + x146;
	const FLT x357 = x262 * x356;
	const FLT x358 = 1.0/2.0 * x357;
	const FLT x359 = x12 * x357;
	const FLT x360 = 1.0/2.0 * x26;
	const FLT x361 = (-1 * x15 * x353) + x147 + (-1 * x252 * x356) + (-1 * x250 * x356) + (-1 * x15 * x355) + (-1 * x253 * x356) + (-1 * x251 * x356) + (x15 * x352);
	const FLT x362 = x255 * x361;
	const FLT x363 = 1.0/2.0 * x362;
	const FLT x364 = -1 * x325;
	const FLT x365 = x12 * x363;
	const FLT x366 = (-1 * x298 * x363) + (-1 * x358 * x300) + x326 + (-1 * x27) + (-1 * x298 * x358) + (x360 * x359) + (x26 * x365) + (-1 * x363 * x300) + x364;
	const FLT x367 = x6 * x94;
	const FLT x368 = x361 * x318;
	const FLT x369 = -1 * x302;
	const FLT x370 = 1.0/2.0 * x4;
	const FLT x371 = x9 * x358;
	const FLT x372 = x6 * x363;
	const FLT x373 = x9 * x363;
	const FLT x374 = (-1 * x26 * x373) + (-1 * x370 * x359) + (-1 * x4 * x365) + x369 + x304 + (x358 * x324) + (x24 * x372) + (-1 * x26 * x371);
	const FLT x375 = x6 * x86;
	const FLT x376 = x9 * x268;
	const FLT x377 = -1 * x274;
	const FLT x378 = x276 + (x86 * x372) + (-1 * x272 * x359) + (-1 * x89 * x365) + (-1 * x376 * x357) + x377 + (x375 * x358) + (-1 * x91 * x373);
	const FLT x379 = x86 * x363;
	const FLT x380 = (-1 * x310) + x311;
	const FLT x381 = x380 + (x91 * x365) + (-1 * x92) + (x268 * x359) + (-1 * x3 * x379) + (-1 * x264 * x358) + (-1 * x89 * x373) + (-1 * x89 * x371);
	const FLT x382 = (x94 * x333) + (-1 * x30 * x333);
	const FLT x383 = (x281 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x259 * (*error_model).Lighthouse.AxisAngleRot[2]);
	const FLT x384 = x382 + (-1 * x381 * x331) + (-1 * x362 * x332) + x383 + (-1 * x6 * x368) + (x362 * x329) + (x362 * x317) + (-1 * x367 * x357) + (x366 * x331) + (-1 * x357 * x332) + (x357 * x317) + (x290 * x378) + (x357 * x329) + (-1 * x290 * x374);
	const FLT x385 = 2 * x22;
	const FLT x386 = x94 * x385;
	const FLT x387 = x30 * x385;
	const FLT x388 = x1 * x27;
	const FLT x389 = x2 * x23;
	const FLT x390 = x8 * x25;
	const FLT x391 = (-1 * x24 * x373) + (-1 * x26 * x372) + (-1 * x287 * x358) + (-1 * x390) + (-1 * x24 * x371) + x389 + x388 + (x286 * x363) + (x286 * x358);
	const FLT x392 = x1 * x92;
	const FLT x393 = x3 * x89;
	const FLT x394 = x8 * x87;
	const FLT x395 = x2 * x90;
	const FLT x396 = (-1 * x91 * x372) + x395 + (x393 * x363) + (-1 * x9 * x379) + (x393 * x358) + (-1 * x394) + (-1 * x291 * x357) + x392 + (-1 * x86 * x371);
	const FLT x397 = x308 + (x396 * x331) + (-1 * x386) + (-1 * x278 * x378) + (-1 * x357 * x336) + (-1 * x357 * x337) + (-1 * x362 * x336) + x387 + (x278 * x374) + (x94 * x359) + (-1 * x362 * x337) + (-1 * x391 * x331) + (x362 * x338) + (x12 * x368) + (x357 * x338);
	const FLT x398 = (x397 * x167) + (x384 * x158);
	const FLT x399 = 2 * x99;
	const FLT x400 = x97 * x385;
	const FLT x401 = (-1 * x261 * x362) + (-1 * x339) + (x97 * x359) + (-1 * x400) + (-1 * x290 * x396) + (-1 * x261 * x357) + (x278 * x381) + (x280 * x362) + (-1 * x98 * x359) + (x290 * x391) + (-1 * x279 * x362) + (x280 * x357) + x340 + x399 + (x257 * x362) + (-1 * x278 * x366);
	const FLT x402 = (-1 * x401 * x108) + (x398 * x185);
	const FLT x403 = ((x384 * x188) + (-1 * x397 * x187)) * x189;
	const FLT x404 = (-1 * x403) + (x402 * x184);
	const FLT x405 = (x401 * x119) + (-1 * x170 * (x398 + (x401 * x152)));
	const FLT x406 = x405 * x175;
	const FLT x407 = (x405 * x177) + (x120 * (x406 + (-1 * x405 * x174)));
	const FLT x408 = (x405 * x179) + (x407 * x120);
	const FLT x409 = x403 + (-1 * x141 * ((-1 * x404 * x208) + (x405 * x207) + (-1 * x204 * ((x202 * ((x408 * x120) + (x405 * x200) + (x405 * x201) + (x120 * (x408 + (x405 * x195) + (x120 * (x407 + (x120 * ((-1 * x405 * x198) + x406 + (x405 * x196))) + (x405 * x199))))))) + (-1 * x404 * x194))) + x402 + (x408 * x181)));
	const FLT x410 = x244 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x411 = x246 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x412 = x354 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x413 = (-1 * x412) + (-1 * x411) + x330 + x410;
	const FLT x414 = (-1 * x15 * x412) + (-1 * x413 * x253) + (-1 * x413 * x251) + (-1 * x413 * x252) + (x15 * x410) + (x15 * x330) + (-1 * x15 * x411) + (-1 * x413 * x250);
	const FLT x415 = x414 * x255;
	const FLT x416 = 1.0/2.0 * x415;
	const FLT x417 = x413 * x262;
	const FLT x418 = x12 * x268;
	const FLT x419 = x9 * x417;
	const FLT x420 = 1.0/2.0 * x417;
	const FLT x421 = -1 * x292;
	const FLT x422 = x296 + x421 + (x417 * x418) + (-1 * x416 * x264) + (-1 * x420 * x264) + (x415 * x418) + (-1 * x415 * x273) + (-1 * x419 * x272);
	const FLT x423 = x86 * x416;
	const FLT x424 = 1.0/2.0 * x419;
	const FLT x425 = (-1 * x90) + (-1 * x417 * x291) + (-1 * x86 * x424) + (-1 * x9 * x423) + (-1 * x415 * x291) + (x417 * x293) + x275 + (x415 * x293) + x377;
	const FLT x426 = x24 * x416;
	const FLT x427 = x6 * x420;
	const FLT x428 = x369 + (x420 * x286) + (-1 * x24 * x424) + x303 + (x416 * x286) + (-1 * x26 * x427) + (-1 * x9 * x426) + (-1 * x416 * x287) + (-1 * x23);
	const FLT x429 = (-1 * x284) + x288;
	const FLT x430 = x429 + (x416 * x299) + (x420 * x299) + (-1 * x416 * x298) + (-1 * x415 * x301) + x25 + (-1 * x420 * x298) + (-1 * x417 * x301);
	const FLT x431 = x98 * x417;
	const FLT x432 = (-1 * x307 * (*error_model).Lighthouse.AxisAngleRot[0]) + (x306 * (*error_model).Lighthouse.AxisAngleRot[0]);
	const FLT x433 = x432 + (x417 * x280) + (-1 * x415 * x261) + (x422 * x278) + (-1 * x417 * x261) + (-1 * x425 * x290) + (x415 * x257) + (x417 * x257) + (x415 * x280) + x383 + (-1 * x415 * x279) + (-1 * x430 * x278) + (x428 * x290) + (-1 * x12 * x431);
	const FLT x434 = x414 * x318;
	const FLT x435 = (x6 * x426) + x390 + (-1 * x26 * x424) + (-1 * x420 * x322) + (-1 * x416 * x322) + (-1 * x388) + x389 + (-1 * x416 * x323) + (x24 * x427);
	const FLT x436 = (-1 * x419 * x268) + (-1 * x392) + (-1 * x415 * x312) + x395 + (-1 * x417 * x312) + (x6 * x423) + (x420 * x375) + x394 + (-1 * x415 * x376);
	const FLT x437 = x341 + (x415 * x329) + (x3 * x431) + (x436 * x290) + (-1 * x417 * x332) + (x430 * x331) + (-1 * x6 * x434) + x400 + (-1 * x399) + (x417 * x317) + (-1 * x422 * x331) + (-1 * x417 * x367) + (-1 * x415 * x332) + (-1 * x435 * x290) + (x415 * x317);
	const FLT x438 = x93 * x385;
	const FLT x439 = x28 * x385;
	const FLT x440 = x94 * x12;
	const FLT x441 = (x417 * x440) + (x417 * x338) + (-1 * x436 * x278) + (-1 * x428 * x331) + (x425 * x331) + (x12 * x434) + (-1 * x417 * x336) + (-1 * x417 * x337) + (-1 * x439) + (-1 * x415 * x336) + x321 + (-1 * x315) + x438 + (x415 * x338) + (x435 * x278) + (-1 * x415 * x337);
	const FLT x442 = (x441 * x167) + (x437 * x158);
	const FLT x443 = (x433 * x119) + (-1 * x170 * (x442 + (x433 * x152)));
	const FLT x444 = x443 * x175;
	const FLT x445 = (x443 * x177) + (x120 * (x444 + (-1 * x443 * x174)));
	const FLT x446 = (x443 * x179) + (x445 * x120);
	const FLT x447 = (-1 * x433 * x108) + (x442 * x185);
	const FLT x448 = ((x437 * x188) + (-1 * x441 * x187)) * x189;
	const FLT x449 = (-1 * x448) + (x447 * x184);
	const FLT x450 = x448 + (-1 * x141 * (x447 + (-1 * x204 * ((x202 * ((x446 * x120) + (x443 * x200) + (x443 * x201) + (x120 * (x446 + (x443 * x195) + (x120 * (x445 + (x120 * ((-1 * x443 * x198) + x444 + (x443 * x196))) + (x443 * x199))))))) + (-1 * x449 * x194))) + (x443 * x207) + (-1 * x449 * x208) + (x446 * x181)));
	const FLT x451 = x244 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x452 = x242 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x453 = x354 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x454 = (-1 * x453) + x159 + (-1 * x451) + x452;
	const FLT x455 = x454 * x262;
	const FLT x456 = x255 * ((-1 * x15 * x453) + (-1 * x454 * x251) + (-1 * x454 * x252) + (-1 * x454 * x250) + (-1 * x15 * x451) + x160 + (x15 * x452) + (-1 * x454 * x253));
	const FLT x457 = x12 * x456;
	const FLT x458 = 1.0/2.0 * x455;
	const FLT x459 = x9 * x456;
	const FLT x460 = x6 * x456;
	const FLT x461 = 1.0/2.0 * x86;
	const FLT x462 = (x461 * x460) + x295 + (-1 * x457 * x272) + (-1 * x455 * x312) + (-1 * x455 * x376) + (-1 * x87) + (x458 * x375) + (-1 * x459 * x268) + x421;
	const FLT x463 = x6 * x455;
	const FLT x464 = 1.0/2.0 * x24;
	const FLT x465 = (-1 * x25) + (x464 * x460) + (-1 * x458 * x322) + (-1 * x459 * x360) + (-1 * x458 * x323) + (-1 * x457 * x370) + x429 + (x464 * x463);
	const FLT x466 = x3 * x456;
	const FLT x467 = x388 + (-1 * x389) + (x457 * x360) + (-1 * x456 * x301) + (-1 * x464 * x466) + (x458 * x299) + (-1 * x455 * x301) + x390 + (-1 * x458 * x298);
	const FLT x468 = x98 * x455;
	const FLT x469 = (-1 * x395) + (-1 * x461 * x466) + (x457 * x268) + x392 + (x455 * x418) + x394 + (-1 * x459 * x272) + (-1 * x458 * x264) + (-1 * x455 * x273);
	const FLT x470 = (-1 * x469 * x331) + (x98 * x466) + (x30 * x460) + (-1 * x387) + x386 + (x462 * x290) + (x3 * x468) + (x455 * x317) + (-1 * x282) + (x467 * x331) + x260 + (-1 * x94 * x460) + (-1 * x455 * x367) + (-1 * x465 * x290) + (-1 * x97 * x466) + (-1 * x455 * x332);
	const FLT x471 = (x455 * x293) + (-1 * x460 * x268) + (-1 * x455 * x291) + x380 + (-1 * x459 * x461) + (x466 * x272) + x92 + (-1 * x9 * x86 * x458);
	const FLT x472 = (-1 * x459 * x464) + x327 + (x458 * x286) + (-1 * x9 * x455 * x464) + (-1 * x460 * x360) + (x466 * x370) + (-1 * x463 * x360) + x364;
	const FLT x473 = x432 + (x94 * x457) + (x465 * x278) + (x471 * x331) + (-1 * x462 * x278) + (-1 * x455 * x337) + (-1 * x455 * x336) + (x455 * x338) + (-1 * x472 * x331) + x382 + (-1 * x93 * x466) + (x455 * x440) + (-1 * x30 * x457) + (x456 * x338);
	const FLT x474 = (x473 * x167) + (x470 * x158);
	const FLT x475 = x334 + x439 + (-1 * x438) + (-1 * x12 * x468) + (x97 * x457) + (-1 * x28 * x460) + (-1 * x471 * x290) + (-1 * x98 * x457) + (x469 * x278) + (-1 * x467 * x278) + (x455 * x257) + (-1 * x455 * x261) + (x472 * x290) + (x455 * x280) + (x93 * x460);
	const FLT x476 = (-1 * x475 * x108) + (x474 * x185);
	const FLT x477 = ((x470 * x188) + (-1 * x473 * x187)) * x189;
	const FLT x478 = x192 * ((-1 * x477) + (x476 * x184));
	const FLT x479 = (x475 * x119) + (-1 * x170 * (x474 + (x475 * x152)));
	const FLT x480 = x479 * x175;
	const FLT x481 = (x479 * x177) + (x120 * (x480 + (-1 * x479 * x174)));
	const FLT x482 = (x479 * x179) + (x481 * x120);
	const FLT x483 = x477 + (-1 * x141 * ((x482 * x181) + x476 + (-1 * x478 * x139) + (-1 * x204 * ((x202 * ((x482 * x120) + (x479 * x200) + (x479 * x201) + (x120 * (x482 + (x479 * x195) + (x120 * (x481 + (x120 * ((-1 * x479 * x198) + (x479 * x196) + x480)) + (x479 * x199))))))) + (-1 * x478 * x193))) + (x479 * x207)));
	const FLT x484 = x32 * x142;
	const FLT x485 = x15 * x484;
	const FLT x486 = x3 * x12 * x485;
	const FLT x487 = x6 * x485;
	const FLT x488 = x9 * x487;
	const FLT x489 = (-1 * x488) + x486;
	const FLT x490 = -1 * x17 * x484;
	const FLT x491 = -1 * x16 * x484;
	const FLT x492 = x491 + x33 + x490;
	const FLT x493 = x9 * x485;
	const FLT x494 = x3 * x493;
	const FLT x495 = x12 * x487;
	const FLT x496 = x495 + x494;
	const FLT x497 = (x496 * x167) + (x492 * x158);
	const FLT x498 = (x489 * x119) + (-1 * x170 * (x497 + (x489 * x152)));
	const FLT x499 = (-1 * x489 * x108) + (x497 * x185);
	const FLT x500 = ((x492 * x188) + (-1 * x496 * x187)) * x189;
	const FLT x501 = (-1 * x500) + (x499 * x184);
	const FLT x502 = x498 * x175;
	const FLT x503 = (x498 * x177) + (x120 * (x502 + (-1 * x498 * x174)));
	const FLT x504 = (x498 * x179) + (x503 * x120);
	const FLT x505 = x500 + (-1 * x141 * ((-1 * x501 * x208) + (x504 * x181) + x499 + (x498 * x207) + (-1 * x204 * ((x202 * ((x504 * x120) + (x498 * x201) + (x120 * (x504 + (x498 * x195) + (x120 * (x503 + (x120 * ((-1 * x498 * x198) + x502 + (x498 * x196))) + (x498 * x199))))) + (x498 * x200))) + (-1 * x501 * x194)))));
	const FLT x506 = (-1 * x18 * x484) + x33;
	const FLT x507 = x506 + x491;
	const FLT x508 = x486 + x488;
	const FLT x509 = x3 * x487;
	const FLT x510 = x12 * x493;
	const FLT x511 = (-1 * x510) + x509;
	const FLT x512 = (x511 * x167) + (x508 * x158);
	const FLT x513 = (x507 * x119) + (-1 * x170 * (x512 + (x507 * x152)));
	const FLT x514 = x513 * x175;
	const FLT x515 = (x513 * x177) + (x120 * (x514 + (-1 * x513 * x174)));
	const FLT x516 = (x513 * x179) + (x515 * x120);
	const FLT x517 = (-1 * x507 * x108) + (x512 * x185);
	const FLT x518 = ((x508 * x188) + (-1 * x511 * x187)) * x189;
	const FLT x519 = (-1 * x518) + (x517 * x184);
	const FLT x520 = x518 + (-1 * x141 * ((-1 * x519 * x208) + (-1 * x204 * ((x202 * ((x516 * x120) + (x513 * x200) + (x120 * (x516 + (x513 * x195) + (x120 * (x515 + (x120 * ((-1 * x513 * x198) + x514 + (x513 * x196))) + (x513 * x199))))) + (x513 * x201))) + (-1 * x519 * x194))) + x517 + (x516 * x181) + (x513 * x207)));
	const FLT x521 = x509 + x510;
	const FLT x522 = (-1 * x494) + x495;
	const FLT x523 = x506 + x490;
	const FLT x524 = (x523 * x167) + (x522 * x158);
	const FLT x525 = (x521 * x119) + (-1 * x170 * (x524 + (x521 * x152)));
	const FLT x526 = x525 * x175;
	const FLT x527 = (x525 * x177) + (x120 * (x526 + (-1 * x525 * x174)));
	const FLT x528 = (x525 * x179) + (x527 * x120);
	const FLT x529 = (-1 * x521 * x108) + (x524 * x185);
	const FLT x530 = ((x522 * x188) + (-1 * x523 * x187)) * x189;
	const FLT x531 = (-1 * x530) + (x529 * x184);
	const FLT x532 = x530 + (-1 * x141 * (x529 + (-1 * x531 * x208) + (-1 * x204 * ((x202 * ((x120 * (x528 + (x525 * x195) + (x120 * (x527 + (x120 * ((-1 * x525 * x198) + x526 + (x525 * x196))) + (x525 * x199))))) + (x528 * x120) + (x525 * x201) + (x525 * x200))) + (-1 * x531 * x194))) + (x528 * x181) + (x525 * x207)));
	const FLT x533 = x214 + x149;
	const FLT x534 = -1 * x154;
	const FLT x535 = -1 * x155;
	const FLT x536 = 1 + x535 + x534;
	const FLT x537 = x162 + x164;
	const FLT x538 = (x537 * x167) + (x536 * x158);
	const FLT x539 = (x533 * x119) + (-1 * x170 * (x538 + (x533 * x152)));
	const FLT x540 = x539 * x175;
	const FLT x541 = (x539 * x177) + (x120 * (x540 + (-1 * x539 * x174)));
	const FLT x542 = (x539 * x179) + (x541 * x120);
	const FLT x543 = (-1 * x533 * x108) + (x538 * x185);
	const FLT x544 = ((x536 * x188) + (-1 * x537 * x187)) * x189;
	const FLT x545 = (-1 * x544) + (x543 * x184);
	const FLT x546 = x544 + (-1 * x141 * (x543 + (x539 * x207) + (x542 * x181) + (-1 * x545 * x208) + (-1 * x204 * ((x202 * ((x542 * x120) + (x539 * x200) + (x539 * x201) + (x120 * (x542 + (x539 * x195) + (x120 * (x541 + (x120 * ((-1 * x539 * x198) + x540 + (x539 * x196))) + (x539 * x199))))))) + (-1 * x545 * x194)))));
	const FLT x547 = x149 + x145;
	const FLT x548 = x232 + x217;
	const FLT x549 = (x548 * x167) + (x547 * x158);
	const FLT x550 = 1 + (-1 * x212);
	const FLT x551 = x550 + x534;
	const FLT x552 = (-1 * x551 * x108) + (x549 * x185);
	const FLT x553 = ((x547 * x188) + (-1 * x548 * x187)) * x189;
	const FLT x554 = (-1 * x553) + (x552 * x184);
	const FLT x555 = (x551 * x119) + (-1 * x170 * (x549 + (x551 * x152)));
	const FLT x556 = x555 * x175;
	const FLT x557 = (x555 * x177) + (x120 * (x556 + (-1 * x555 * x174)));
	const FLT x558 = (x555 * x179) + (x557 * x120);
	const FLT x559 = x553 + (-1 * x141 * (x552 + (-1 * x554 * x208) + (-1 * x204 * ((x202 * ((x555 * x200) + (x558 * x120) + (x555 * x201) + (x120 * (x558 + (x555 * x195) + (x120 * (x557 + (x120 * ((-1 * x555 * x198) + x556 + (x555 * x196))) + (x555 * x199))))))) + (-1 * x554 * x194))) + (x555 * x207) + (x558 * x181)));
	const FLT x560 = x217 + x216;
	const FLT x561 = x165 + x162;
	const FLT x562 = x550 + x535;
	const FLT x563 = (x562 * x167) + (x561 * x158);
	const FLT x564 = (x560 * x119) + (-1 * x170 * (x563 + (x560 * x152)));
	const FLT x565 = x564 * x175;
	const FLT x566 = (x564 * x177) + (x120 * (x565 + (-1 * x564 * x174)));
	const FLT x567 = (x564 * x179) + (x566 * x120);
	const FLT x568 = (-1 * x560 * x108) + (x563 * x185);
	const FLT x569 = ((x561 * x188) + (-1 * x562 * x187)) * x189;
	const FLT x570 = (-1 * x569) + (x568 * x184);
	const FLT x571 = x569 + (-1 * x141 * (x568 + (-1 * x570 * x208) + (-1 * x204 * ((x202 * ((x567 * x120) + (x564 * x200) + (x564 * x201) + (x120 * (x567 + (x564 * x195) + (x120 * (x566 + (x120 * ((-1 * x564 * x198) + x565 + (x564 * x196))) + (x564 * x199))))))) + (-1 * x570 * x194))) + (x567 * x181) + (x564 * x207)));
	const FLT x572 = 1.0/2.0 * x41;
	const FLT x573 = 1.0 * x42;
	const FLT x574 = 1.0 * x40;
	const FLT x575 = 1.0 * x41;
	const FLT x576 = (x575 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x574 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x573 * (*error_model).Object.Pose.AxisAngleRot[0]) + (2 * x38);
	const FLT x577 = 1. / (x43 * sqrt(x43));
	const FLT x578 = x66 * x577;
	const FLT x579 = x576 * x578;
	const FLT x580 = x48 * x579;
	const FLT x581 = 1.0/2.0 * x40;
	const FLT x582 = x70 * x577;
	const FLT x583 = x576 * x582;
	const FLT x584 = 1.0/2.0 * x38;
	const FLT x585 = x34 * x584;
	const FLT x586 = x67 * x48;
	const FLT x587 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x588 = x586 * x587;
	const FLT x589 = 1.0/2.0 * x42;
	const FLT x590 = x51 * x579;
	const FLT x591 = 0.5 * x71;
	const FLT x592 = x591 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x593 = x67 * x34;
	const FLT x594 = x67 * x51;
	const FLT x595 = 0.5 * x594;
	const FLT x596 = -1 * x595 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x597 = x596 + x593;
	const FLT x598 = x597 + (-1 * x581 * x583) + (x590 * x589) + (-1 * x572 * x580) + x592 + (-1 * x579 * x585) + x588;
	const FLT x599 = 2 * x82;
	const FLT x600 = x34 * x579;
	const FLT x601 = 1.0/2.0 * x583;
	const FLT x602 = x591 * (*error_model).Object.Pose.AxisAngleRot[1];
	const FLT x603 = 0.5 * x586;
	const FLT x604 = -1 * x603 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x605 = 0.5 * x593;
	const FLT x606 = x605 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x607 = x606 + x604;
	const FLT x608 = x607 + x602 + (-1 * x600 * x589) + (x580 * x581) + x594 + (-1 * x590 * x584) + (-1 * x41 * x601);
	const FLT x609 = x594 * x587;
	const FLT x610 = -1 * x609;
	const FLT x611 = x605 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x612 = -1 * x611;
	const FLT x613 = x603 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x614 = (-1 * x613) + x71;
	const FLT x615 = x614 + (x590 * x572) + x612 + x610 + (x600 * x581) + (-1 * x38 * x601) + (x580 * x589);
	const FLT x616 = x595 * (*error_model).Object.Pose.AxisAngleRot[2];
	const FLT x617 = -1 * x593 * x587;
	const FLT x618 = x591 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x619 = x618 + x617;
	const FLT x620 = x616 + (-1 * x590 * x581) + (-1 * x580 * x584) + x619 + x586 + (x600 * x572) + (-1 * x589 * x583);
	const FLT x621 = (x620 * sensor_pt[1]) + (-1 * x608 * sensor_pt[0]) + (x615 * sensor_pt[2]);
	const FLT x622 = 2 * x84;
	const FLT x623 = (x598 * sensor_pt[0]) + (-1 * x620 * sensor_pt[2]) + (x615 * sensor_pt[1]);
	const FLT x624 = 2 * x75;
	const FLT x625 = 2 * x85;
	const FLT x626 = (x625 * x608) + (-1 * x624 * x623) + (-1 * x599 * x598) + (x622 * x621);
	const FLT x627 = 2 * x88;
	const FLT x628 = (x608 * sensor_pt[2]) + (x615 * sensor_pt[0]) + (-1 * x598 * sensor_pt[1]);
	const FLT x629 = 2 * x81;
	const FLT x630 = (x623 * x629) + (x620 * x599) + (-1 * x627 * x608) + (-1 * x622 * x628);
	const FLT x631 = x9 * x22;
	const FLT x632 = (x627 * x598) + (x624 * x628) + (-1 * x625 * x620) + (-1 * x621 * x629);
	const FLT x633 = x22 * x632;
	const FLT x634 = (-1 * x12 * x633) + (x29 * x626) + (x630 * x631);
	const FLT x635 = (x31 * x630) + (-1 * x96 * x626) + (x9 * x633);
	const FLT x636 = x626 + (-1 * x634 * x331) + (x635 * x290);
	const FLT x637 = (-1 * x29 * x630) + (x626 * x631) + (x96 * x632);
	const FLT x638 = x630 + (x637 * x331) + (-1 * x635 * x278);
	const FLT x639 = (x638 * x167) + (x636 * x158);
	const FLT x640 = (x634 * x278) + x632 + (-1 * x637 * x290);
	const FLT x641 = (-1 * x640 * x108) + (x639 * x185);
	const FLT x642 = ((x636 * x188) + (-1 * x638 * x187)) * x189;
	const FLT x643 = (-1 * x642) + (x641 * x184);
	const FLT x644 = (x640 * x119) + (-1 * x170 * (x639 + (x640 * x152)));
	const FLT x645 = x644 * x175;
	const FLT x646 = (x644 * x177) + (x120 * (x645 + (-1 * x644 * x174)));
	const FLT x647 = (x644 * x179) + (x646 * x120);
	const FLT x648 = x642 + (-1 * x141 * (x641 + (-1 * x643 * x208) + (x647 * x181) + (-1 * x204 * ((x202 * ((x647 * x120) + (x644 * x200) + (x644 * x201) + (x120 * (x647 + (x644 * x195) + (x120 * (x646 + (x120 * ((-1 * x644 * x198) + x645 + (x644 * x196))) + (x644 * x199))))))) + (-1 * x643 * x194))) + (x644 * x207)));
	const FLT x649 = 1.0 * x38;
	const FLT x650 = (2 * x42) + (-1 * x575 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x649 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x574 * (*error_model).Object.Pose.AxisAngleRot[1]);
	const FLT x651 = x650 * x578;
	const FLT x652 = x48 * x651;
	const FLT x653 = x51 * x651;
	const FLT x654 = x650 * x582;
	const FLT x655 = x34 * x651;
	const FLT x656 = x614 + (x655 * x572) + x609 + (-1 * x652 * x584) + x611 + (-1 * x653 * x581) + (-1 * x654 * x589);
	const FLT x657 = -1 * x588;
	const FLT x658 = -1 * x592;
	const FLT x659 = (-1 * x654 * x572) + (-1 * x653 * x584) + (x652 * x581) + x657 + (-1 * x655 * x589) + x597 + x658;
	const FLT x660 = -1 * x586;
	const FLT x661 = (-1 * x618) + x617;
	const FLT x662 = (x653 * x572) + (-1 * x654 * x584) + (x652 * x589) + x616 + x660 + x661 + (x655 * x581);
	const FLT x663 = (-1 * x659 * sensor_pt[0]) + (x656 * sensor_pt[1]) + (x662 * sensor_pt[2]);
	const FLT x664 = -1 * x594;
	const FLT x665 = (-1 * x606) + x604;
	const FLT x666 = (-1 * x654 * x581) + x602 + x664 + (-1 * x651 * x585) + (-1 * x652 * x572) + x665 + (x653 * x589);
	const FLT x667 = (x659 * sensor_pt[2]) + (-1 * x666 * sensor_pt[1]) + (x662 * sensor_pt[0]);
	const FLT x668 = (x627 * x666) + (-1 * x625 * x656) + (x624 * x667) + (-1 * x629 * x663);
	const FLT x669 = (x662 * sensor_pt[1]) + (x666 * sensor_pt[0]) + (-1 * x656 * sensor_pt[2]);
	const FLT x670 = (-1 * x624 * x669) + (-1 * x666 * x599) + (x625 * x659) + (x622 * x663);
	const FLT x671 = (x629 * x669) + (x656 * x599) + (-1 * x627 * x659) + (-1 * x622 * x667);
	const FLT x672 = (-1 * x29 * x671) + (x96 * x668) + (x631 * x670);
	const FLT x673 = (-1 * x31 * x668) + (x29 * x670) + (x631 * x671);
	const FLT x674 = x668 + (-1 * x672 * x290) + (x673 * x278);
	const FLT x675 = (-1 * x96 * x670) + (x31 * x671) + (x631 * x668);
	const FLT x676 = x670 + (x675 * x290) + (-1 * x673 * x331);
	const FLT x677 = x671 + (x672 * x331) + (-1 * x675 * x278);
	const FLT x678 = (x677 * x167) + (x676 * x158);
	const FLT x679 = (x674 * x119) + (-1 * x170 * (x678 + (x674 * x152)));
	const FLT x680 = x679 * x173;
	const FLT x681 = x679 * x175;
	const FLT x682 = (x679 * x177) + (x120 * (x681 + (-1 * x680 * x121)));
	const FLT x683 = (x679 * x179) + (x682 * x120);
	const FLT x684 = (-1 * x674 * x108) + (x678 * x185);
	const FLT x685 = ((x676 * x188) + (-1 * x677 * x187)) * x189;
	const FLT x686 = (-1 * x685) + (x684 * x184);
	const FLT x687 = x685 + (-1 * x141 * (x684 + (-1 * x686 * x208) + (-1 * x204 * ((x202 * ((x683 * x120) + (x120 * (x683 + (x679 * x195) + (x120 * (x682 + (x120 * ((-1 * x680 * x197) + x681 + (x679 * x196))) + (x679 * x199))))) + (x679 * x201) + (x679 * x200))) + (-1 * x686 * x194))) + (x683 * x181) + (x680 * x206 * x128 * x137)));
	const FLT x688 = (2 * x41) + (x573 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x649 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x574 * (*error_model).Object.Pose.AxisAngleRot[0]);
	const FLT x689 = x688 * x578;
	const FLT x690 = x48 * x689;
	const FLT x691 = x34 * x689;
	const FLT x692 = x51 * x689;
	const FLT x693 = x688 * x582;
	const FLT x694 = 1.0/2.0 * x693;
	const FLT x695 = x71 + x613;
	const FLT x696 = x695 + (-1 * x691 * x589) + (x690 * x581) + (-1 * x692 * x584) + x610 + x611 + (-1 * x41 * x694);
	const FLT x697 = (-1 * x593) + x596;
	const FLT x698 = x697 + (-1 * x692 * x581) + x592 + (-1 * x690 * x584) + x657 + (-1 * x693 * x589) + (x691 * x572);
	const FLT x699 = -1 * x616;
	const FLT x700 = x661 + (-1 * x689 * x585) + (-1 * x693 * x581) + (-1 * x690 * x572) + x586 + x699 + (x692 * x589);
	const FLT x701 = -1 * x602;
	const FLT x702 = x607 + x701 + (x691 * x581) + (-1 * x38 * x694) + x664 + (x690 * x589) + (x692 * x572);
	const FLT x703 = (x696 * sensor_pt[2]) + (-1 * x700 * sensor_pt[1]) + (x702 * sensor_pt[0]);
	const FLT x704 = (x700 * sensor_pt[0]) + (-1 * x698 * sensor_pt[2]) + (x702 * sensor_pt[1]);
	const FLT x705 = (x629 * x704) + (-1 * x622 * x703) + (-1 * x696 * x627) + (x698 * x599);
	const FLT x706 = (x698 * sensor_pt[1]) + (-1 * x696 * sensor_pt[0]) + (x702 * sensor_pt[2]);
	const FLT x707 = (x627 * x700) + (-1 * x629 * x706) + (-1 * x698 * x625) + (x624 * x703);
	const FLT x708 = x22 * x707;
	const FLT x709 = (-1 * x624 * x704) + (x696 * x625) + (-1 * x700 * x599) + (x622 * x706);
	const FLT x710 = (-1 * x96 * x709) + (x31 * x705) + (x9 * x708);
	const FLT x711 = x22 * ((-1 * x12 * x708) + (x29 * x709) + (x631 * x705));
	const FLT x712 = x709 + (x710 * x290) + (-1 * x711 * x330);
	const FLT x713 = (x6 * x708) + (-1 * x29 * x705) + (x631 * x709);
	const FLT x714 = (x713 * x331) + x705 + (-1 * x710 * x278);
	const FLT x715 = (x714 * x167) + (x712 * x158);
	const FLT x716 = x707 + (x711 * x146) + (-1 * x713 * x290);
	const FLT x717 = (-1 * x716 * x108) + (x715 * x185);
	const FLT x718 = ((x712 * x188) + (-1 * x714 * x187)) * x189;
	const FLT x719 = (-1 * x718) + (x717 * x184);
	const FLT x720 = (x716 * x119) + (-1 * x170 * (x715 + (x716 * x152)));
	const FLT x721 = x720 * x175;
	const FLT x722 = (x720 * x177) + (x120 * (x721 + (-1 * x720 * x174)));
	const FLT x723 = (x720 * x179) + (x722 * x120);
	const FLT x724 = x718 + (-1 * x141 * ((-1 * x719 * x208) + (x723 * x181) + (-1 * x204 * ((x202 * ((x720 * x200) + (x723 * x120) + (x720 * x201) + (x120 * (x723 + (x720 * x195) + (x120 * (x722 + (x120 * ((-1 * x720 * x198) + x721 + (x720 * x196))) + (x720 * x199))))))) + (-1 * x719 * x194))) + x717 + (x720 * x207)));
	const FLT x725 = (x575 * (*error_model).Object.Pose.AxisAngleRot[0]) + (2 * x40) + (-1 * x573 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x649 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x726 = x725 * x578;
	const FLT x727 = x48 * x726;
	const FLT x728 = x34 * x726;
	const FLT x729 = x725 * x582;
	const FLT x730 = x51 * x726;
	const FLT x731 = (-1 * x730 * x581) + x594 + x701 + (-1 * x727 * x584) + x665 + (-1 * x729 * x589) + (x728 * x572);
	const FLT x732 = 1.0/2.0 * x729;
	const FLT x733 = x697 + (x730 * x572) + x658 + (x727 * x589) + x588 + (-1 * x38 * x732) + (x728 * x581);
	const FLT x734 = (-1 * x41 * x732) + (-1 * x730 * x584) + x699 + x660 + (-1 * x728 * x589) + x619 + (x727 * x581);
	const FLT x735 = (x731 * sensor_pt[1]) + (x733 * sensor_pt[2]) + (-1 * x734 * sensor_pt[0]);
	const FLT x736 = x695 + x609 + (-1 * x726 * x585) + (-1 * x40 * x732) + (-1 * x727 * x572) + (x730 * x589) + x612;
	const FLT x737 = (x734 * sensor_pt[2]) + (-1 * x736 * sensor_pt[1]) + (x733 * sensor_pt[0]);
	const FLT x738 = (x627 * x736) + (x624 * x737) + (-1 * x625 * x731) + (-1 * x629 * x735);
	const FLT x739 = (x736 * sensor_pt[0]) + (-1 * x731 * sensor_pt[2]) + (x733 * sensor_pt[1]);
	const FLT x740 = (x625 * x734) + (-1 * x624 * x739) + (-1 * x736 * x599) + (x622 * x735);
	const FLT x741 = (x629 * x739) + (x731 * x599) + (-1 * x627 * x734) + (-1 * x622 * x737);
	const FLT x742 = (-1 * x29 * x741) + (x96 * x738) + (x631 * x740);
	const FLT x743 = (-1 * x31 * x738) + (x29 * x740) + (x631 * x741);
	const FLT x744 = x738 + (-1 * x742 * x290) + (x743 * x278);
	const FLT x745 = (-1 * x96 * x740) + (x31 * x741) + (x631 * x738);
	const FLT x746 = x740 + (x745 * x290) + (-1 * x743 * x331);
	const FLT x747 = x741 + (x742 * x331) + (-1 * x745 * x278);
	const FLT x748 = (x747 * x167) + (x746 * x158);
	const FLT x749 = (x744 * x119) + (-1 * x170 * (x748 + (x744 * x152)));
	const FLT x750 = x749 * x175;
	const FLT x751 = (x749 * x177) + (x120 * (x750 + (-1 * x749 * x174)));
	const FLT x752 = (x749 * x179) + (x751 * x120);
	const FLT x753 = (-1 * x744 * x108) + (x748 * x185);
	const FLT x754 = ((x746 * x188) + (-1 * x747 * x187)) * x189;
	const FLT x755 = (-1 * x754) + (x753 * x184);
	const FLT x756 = x754 + (-1 * x141 * (x753 + (x749 * x207) + (-1 * x755 * x208) + (-1 * x204 * ((x202 * ((x752 * x120) + (x749 * x200) + (x749 * x201) + (x120 * (x752 + (x749 * x195) + (x120 * (x751 + (x120 * (x750 + (-1 * x749 * x198) + (x749 * x196))) + (x749 * x199))))))) + (-1 * x755 * x194))) + (x752 * x181)));
	const FLT x757 = dt * dt * dt;
	const FLT x758 = x49 * x757;
	const FLT x759 = 0.5 * x59;
	const FLT x760 = x758 * x759;
	const FLT x761 = x44 * x589;
	const FLT x762 = dt * dt * dt * dt;
	const FLT x763 = (x48 * x48 * x48) * x762;
	const FLT x764 = 2 * (1. / (x54 * x54)) * x58;
	const FLT x765 = 1.0 * x61;
	const FLT x766 = (1. / (x54 * sqrt(x54))) * x57;
	const FLT x767 = x766 * x765;
	const FLT x768 = x767 * x762;
	const FLT x769 = x48 * x768;
	const FLT x770 = x764 * x762;
	const FLT x771 = x48 * x770;
	const FLT x772 = 2 * x60;
	const FLT x773 = x45 * x772;
	const FLT x774 = x64 * x765;
	const FLT x775 = x45 * x48;
	const FLT x776 = (x767 * x763) + (x52 * x769) + (-1 * x774 * x775) + (-1 * x764 * x763) + (x46 * x769) + (-1 * x52 * x771) + (x48 * x773) + (-1 * x46 * x771);
	const FLT x777 = 1. / (x62 * sqrt(x62));
	const FLT x778 = dt * x64 * x777;
	const FLT x779 = x778 * x776;
	const FLT x780 = x51 * x779;
	const FLT x781 = x44 * x38;
	const FLT x782 = 1.0/2.0 * x781;
	const FLT x783 = x34 * x779;
	const FLT x784 = 1.0/2.0 * x73;
	const FLT x785 = x48 * x779;
	const FLT x786 = 0.5 * x65;
	const FLT x787 = x775 * x786;
	const FLT x788 = x61 * x777;
	const FLT x789 = x776 * x788;
	const FLT x790 = 1.0/2.0 * x789;
	const FLT x791 = x766 * x758;
	const FLT x792 = x63 * x791;
	const FLT x793 = x766 * x757;
	const FLT x794 = x48 * x793;
	const FLT x795 = x51 * x794;
	const FLT x796 = x42 * x44;
	const FLT x797 = x63 * x796;
	const FLT x798 = x759 * x757;
	const FLT x799 = x51 * x798;
	const FLT x800 = x48 * x799;
	const FLT x801 = (-1 * x80 * x800) + (x795 * x797);
	const FLT x802 = x34 * x798;
	const FLT x803 = x48 * x802;
	const FLT x804 = x63 * x781;
	const FLT x805 = (-1 * x34 * x794 * x804) + (x78 * x803);
	const FLT x806 = x801 + (-1 * x76 * x790) + (-1 * x76 * x787) + x74 + (x761 * x780) + x805 + (x83 * x760) + (-1 * x782 * x783) + (-1 * x73 * x792) + (-1 * x784 * x785);
	const FLT x807 = -1 * x77;
	const FLT x808 = x34 * x797;
	const FLT x809 = x794 * x808;
	const FLT x810 = x76 * x63;
	const FLT x811 = x80 * x803;
	const FLT x812 = 1.0/2.0 * x76;
	const FLT x813 = (-1 * x795 * x804) + (x78 * x800);
	const FLT x814 = x813 + x811 + (-1 * x809) + (-1 * x789 * x784) + (x791 * x810) + (-1 * x73 * x787) + (-1 * x761 * x783) + x807 + (x785 * x812) + (-1 * x72 * x760) + (-1 * x782 * x780);
	const FLT x815 = x73 * x63;
	const FLT x816 = x795 * x815;
	const FLT x817 = x83 * x48;
	const FLT x818 = x799 * x817;
	const FLT x819 = -1 * x69;
	const FLT x820 = x72 * x802;
	const FLT x821 = x794 * x810;
	const FLT x822 = (x34 * x821) + (-1 * x48 * x820);
	const FLT x823 = x822 + x819 + (-1 * x781 * x787) + (x761 * x785) + (-1 * x80 * x760) + (-1 * x790 * x781) + (x792 * x796) + (x784 * x780) + (x783 * x812) + (-1 * x818) + x816;
	const FLT x824 = x802 * x817;
	const FLT x825 = x72 * x800;
	const FLT x826 = x34 * x784;
	const FLT x827 = x51 * x821;
	const FLT x828 = x34 * x815;
	const FLT x829 = x794 * x828;
	const FLT x830 = (-1 * x781 * x792) + x68 + (-1 * x780 * x812) + (-1 * x824) + (-1 * x782 * x785) + (x779 * x826) + (-1 * x787 * x796) + x829 + (x78 * x760) + x825 + (-1 * x761 * x789) + (-1 * x827);
	const FLT x831 = (x830 * sensor_pt[1]) + (-1 * x814 * sensor_pt[0]) + (x823 * sensor_pt[2]);
	const FLT x832 = (-1 * x830 * sensor_pt[2]) + (x806 * sensor_pt[0]) + (x823 * sensor_pt[1]);
	const FLT x833 = (-1 * x624 * x832) + (x625 * x814) + (-1 * x599 * x806) + (x622 * x831);
	const FLT x834 = (-1 * x806 * sensor_pt[1]) + (x814 * sensor_pt[2]) + (x823 * sensor_pt[0]);
	const FLT x835 = (x629 * x832) + (x599 * x830) + (-1 * x627 * x814) + (-1 * x622 * x834);
	const FLT x836 = (-1 * x629 * x831) + (x624 * x834) + (-1 * x625 * x830) + (x627 * x806);
	const FLT x837 = x22 * x836;
	const FLT x838 = (-1 * x12 * x837) + (x29 * x833) + (x631 * x835);
	const FLT x839 = (-1 * x29 * x835) + (x96 * x836) + (x631 * x833);
	const FLT x840 = x836 + (x838 * x278) + (-1 * x839 * x290);
	const FLT x841 = (-1 * x96 * x833) + (x31 * x835) + (x9 * x837);
	const FLT x842 = x833 + (x841 * x290) + (-1 * x838 * x331);
	const FLT x843 = x835 + (x839 * x331) + (-1 * x841 * x278);
	const FLT x844 = (x843 * x167) + (x842 * x158);
	const FLT x845 = (x840 * x119) + (-1 * x170 * (x844 + (x840 * x152)));
	const FLT x846 = (-1 * x840 * x108) + (x844 * x185);
	const FLT x847 = ((x842 * x188) + (-1 * x843 * x187)) * x189;
	const FLT x848 = (-1 * x847) + (x846 * x184);
	const FLT x849 = x845 * x175;
	const FLT x850 = (x845 * x177) + (x120 * (x849 + (-1 * x845 * x174)));
	const FLT x851 = (x845 * x179) + (x850 * x120);
	const FLT x852 = x847 + (-1 * x141 * ((x851 * x181) + x846 + (-1 * x848 * x208) + (x845 * x207) + (-1 * x204 * ((x202 * ((x851 * x120) + (x845 * x200) + (x845 * x201) + (x120 * (x851 + (x845 * x195) + (x120 * (x850 + (x120 * ((-1 * x845 * x198) + x849 + (x845 * x196))) + (x845 * x199))))))) + (-1 * x848 * x194)))));
	const FLT x853 = x52 * x798;
	const FLT x854 = x52 * x793;
	const FLT x855 = x63 * x854;
	const FLT x856 = x49 * x770;
	const FLT x857 = x49 * x768;
	const FLT x858 = x51 * x45;
	const FLT x859 = x64 * x858;
	const FLT x860 = x51 * x46;
	const FLT x861 = x51 * x51 * x51;
	const FLT x862 = (x772 * x858) + (-1 * x770 * x861) + (x768 * x861) + (-1 * x51 * x856) + (-1 * x765 * x859) + (x51 * x857) + (x768 * x860) + (-1 * x770 * x860);
	const FLT x863 = x778 * x862;
	const FLT x864 = x48 * x863;
	const FLT x865 = x34 * x863;
	const FLT x866 = x51 * x863;
	const FLT x867 = x788 * x862;
	const FLT x868 = 0.5 * x859;
	const FLT x869 = x51 * x793;
	const FLT x870 = x34 * x869;
	const FLT x871 = x51 * x802;
	const FLT x872 = (x78 * x871) + (-1 * x870 * x804);
	const FLT x873 = x872 + (-1 * x810 * x868) + (-1 * x816) + x819 + (-1 * x784 * x864) + (-1 * x782 * x865) + (-1 * x80 * x853) + (x796 * x855) + (-1 * x812 * x867) + (x761 * x866) + x818;
	const FLT x874 = x808 * x869;
	const FLT x875 = x80 * x871;
	const FLT x876 = x812 * x863;
	const FLT x877 = (x78 * x853) + (x48 * x876) + (-1 * x784 * x867) + (-1 * x874) + x68 + (-1 * x825) + (-1 * x804 * x854) + (-1 * x761 * x865) + x827 + (-1 * x782 * x866) + (-1 * x815 * x868) + x875;
	const FLT x878 = x870 * x810;
	const FLT x879 = x51 * x820;
	const FLT x880 = -1 * x74;
	const FLT x881 = x801 + (x784 * x866) + (-1 * x804 * x868) + (x34 * x876) + (x761 * x864) + (-1 * x782 * x867) + x880 + x878 + (-1 * x83 * x853) + (x73 * x855) + (-1 * x879);
	const FLT x882 = (-1 * x83 * x871) + (x828 * x869);
	const FLT x883 = x813 + x882 + (-1 * x797 * x868) + x77 + (-1 * x51 * x876) + (-1 * x810 * x854) + (-1 * x761 * x867) + (x72 * x853) + (x826 * x863) + (-1 * x782 * x864);
	const FLT x884 = (x883 * sensor_pt[1]) + (-1 * x877 * sensor_pt[0]) + (x881 * sensor_pt[2]);
	const FLT x885 = (-1 * x883 * sensor_pt[2]) + (x873 * sensor_pt[0]) + (x881 * sensor_pt[1]);
	const FLT x886 = (x625 * x877) + (-1 * x599 * x873) + (-1 * x624 * x885) + (x622 * x884);
	const FLT x887 = (x877 * sensor_pt[2]) + (-1 * x873 * sensor_pt[1]) + (x881 * sensor_pt[0]);
	const FLT x888 = (x624 * x887) + (-1 * x629 * x884) + (-1 * x625 * x883) + (x627 * x873);
	const FLT x889 = (-1 * x622 * x887) + (x629 * x885) + (-1 * x627 * x877) + (x599 * x883);
	const FLT x890 = (-1 * x29 * x889) + (x631 * x886) + (x96 * x888);
	const FLT x891 = (-1 * x31 * x888) + (x29 * x886) + (x631 * x889);
	const FLT x892 = x888 + (-1 * x890 * x290) + (x891 * x278);
	const FLT x893 = x22 * ((-1 * x96 * x886) + (x31 * x889) + (x631 * x888));
	const FLT x894 = x886 + (x893 * x159) + (-1 * x891 * x331);
	const FLT x895 = x889 + (x890 * x331) + (-1 * x893 * x146);
	const FLT x896 = (x895 * x167) + (x894 * x158);
	const FLT x897 = (x892 * x119) + (-1 * x170 * (x896 + (x892 * x152)));
	const FLT x898 = x897 * x175;
	const FLT x899 = (x897 * x177) + (x120 * (x898 + (-1 * x897 * x174)));
	const FLT x900 = (x897 * x179) + (x899 * x120);
	const FLT x901 = (-1 * x892 * x108) + (x896 * x185);
	const FLT x902 = ((x894 * x188) + (-1 * x895 * x187)) * x189;
	const FLT x903 = (-1 * x902) + (x901 * x184);
	const FLT x904 = x902 + (-1 * x141 * (x901 + (-1 * x903 * x208) + (-1 * x204 * ((x202 * ((x900 * x120) + (x120 * (x900 + (x897 * x195) + (x120 * (x899 + (x120 * (x898 + (-1 * x897 * x198) + (x897 * x196))) + (x897 * x199))))) + (x897 * x201) + (x897 * x200))) + (-1 * x903 * x194))) + (x900 * x181) + (x897 * x207)));
	const FLT x905 = x45 * x34;
	const FLT x906 = x52 * x34;
	const FLT x907 = x34 * x34 * x34;
	const FLT x908 = (x907 * x768) + (-1 * x907 * x770) + (-1 * x34 * x856) + (x34 * x857) + (x906 * x768) + (-1 * x905 * x774) + (x34 * x773) + (-1 * x906 * x770);
	const FLT x909 = x908 * x778;
	const FLT x910 = x34 * x909;
	const FLT x911 = x46 * x793;
	const FLT x912 = x51 * x909;
	const FLT x913 = x46 * x798;
	const FLT x914 = x905 * x786;
	const FLT x915 = x908 * x788;
	const FLT x916 = x48 * x909;
	const FLT x917 = (-1 * x916 * x784) + (-1 * x910 * x782) + x824 + x874 + x68 + (x912 * x761) + (-1 * x875) + (-1 * x911 * x804) + (x78 * x913) + (-1 * x829) + (-1 * x76 * x914) + (-1 * x915 * x812);
	const FLT x918 = x63 * x911;
	const FLT x919 = x805 + x879 + (x909 * x826) + (-1 * x912 * x812) + (-1 * x916 * x782) + (-1 * x915 * x761) + (-1 * x878) + x880 + (x73 * x918) + (-1 * x914 * x796) + (-1 * x83 * x913);
	const FLT x920 = x882 + (-1 * x915 * x782) + (-1 * x914 * x781) + (x911 * x810) + (x910 * x812) + (x912 * x784) + x807 + (-1 * x811) + (x916 * x761) + (-1 * x72 * x913) + x809;
	const FLT x921 = (x917 * sensor_pt[0]) + (-1 * x919 * sensor_pt[2]) + (x920 * sensor_pt[1]);
	const FLT x922 = x822 + x872 + (-1 * x918 * x796) + (-1 * x915 * x784) + (-1 * x73 * x914) + x69 + (x916 * x812) + (x80 * x913) + (-1 * x912 * x782) + (-1 * x910 * x761);
	const FLT x923 = (x919 * sensor_pt[1]) + (x920 * sensor_pt[2]) + (-1 * x922 * sensor_pt[0]);
	const FLT x924 = (x923 * x622) + (x922 * x625) + (-1 * x917 * x599) + (-1 * x921 * x624);
	const FLT x925 = (x922 * sensor_pt[2]) + (-1 * x917 * sensor_pt[1]) + (x920 * sensor_pt[0]);
	const FLT x926 = (-1 * x925 * x622) + (x921 * x629) + (-1 * x922 * x627) + (x919 * x599);
	const FLT x927 = (x925 * x624) + (-1 * x923 * x629) + (-1 * x919 * x625) + (x917 * x627);
	const FLT x928 = x22 * x927;
	const FLT x929 = (-1 * x12 * x928) + (x29 * x924) + (x926 * x631);
	const FLT x930 = (x6 * x928) + (-1 * x29 * x926) + (x924 * x631);
	const FLT x931 = x927 + (x929 * x278) + (-1 * x930 * x290);
	const FLT x932 = (-1 * x96 * x924) + (x31 * x926) + (x9 * x928);
	const FLT x933 = x924 + (x932 * x290) + (-1 * x929 * x331);
	const FLT x934 = x926 + (x930 * x331) + (-1 * x932 * x278);
	const FLT x935 = (x934 * x167) + (x933 * x158);
	const FLT x936 = (x931 * x119) + (-1 * x170 * (x935 + (x931 * x152)));
	const FLT x937 = x936 * x175;
	const FLT x938 = (x936 * x177) + (x120 * (x937 + (-1 * x936 * x174)));
	const FLT x939 = (x936 * x179) + (x938 * x120);
	const FLT x940 = (-1 * x931 * x108) + (x935 * x185);
	const FLT x941 = ((x933 * x188) + (-1 * x934 * x187)) * x189;
	const FLT x942 = (-1 * x941) + (x940 * x184);
	const FLT x943 = x941 + (-1 * x141 * (x940 + (-1 * x204 * ((x202 * ((x939 * x120) + (x120 * (x939 + (x936 * x195) + (x120 * (x938 + (x120 * ((-1 * x936 * x198) + (x936 * x196) + x937)) + (x936 * x199))))) + (x936 * x201) + (x936 * x200))) + (-1 * x942 * x194))) + (-1 * x942 * x208) + (x936 * x207) + (x939 * x181)));
	const FLT x944 = -1 * dt * x155;
	const FLT x945 = -1 * dt * x154;
	const FLT x946 = x945 + x944 + dt;
	const FLT x947 = dt * x164;
	const FLT x948 = dt * x162;
	const FLT x949 = x948 + x947;
	const FLT x950 = (x949 * x167) + (x946 * x158);
	const FLT x951 = dt * x149;
	const FLT x952 = dt * x145;
	const FLT x953 = (-1 * x952) + x951;
	const FLT x954 = (-1 * x953 * x108) + (x950 * x185);
	const FLT x955 = ((x946 * x188) + (-1 * x949 * x187)) * x189;
	const FLT x956 = (-1 * x955) + (x954 * x184);
	const FLT x957 = (x953 * x119) + (-1 * x170 * (x950 + (x953 * x152)));
	const FLT x958 = x957 * x175;
	const FLT x959 = (x957 * x177) + (x120 * (x958 + (-1 * x957 * x174)));
	const FLT x960 = (x957 * x179) + (x959 * x120);
	const FLT x961 = x955 + (-1 * x141 * ((-1 * x956 * x208) + (x957 * x207) + x954 + (-1 * x204 * ((x202 * ((x957 * x200) + (x120 * (x960 + (x957 * x195) + (x120 * (x959 + (x120 * ((-1 * x957 * x198) + x958 + (x957 * x196))) + (x957 * x199))))) + (x960 * x120) + (x957 * x201))) + (-1 * x956 * x194))) + (x960 * x181)));
	const FLT x962 = (-1 * dt * x212) + dt;
	const FLT x963 = x962 + x945;
	const FLT x964 = x951 + x952;
	const FLT x965 = dt * x217;
	const FLT x966 = dt * x216;
	const FLT x967 = (-1 * x966) + x965;
	const FLT x968 = (x967 * x167) + (x964 * x158);
	const FLT x969 = (x963 * x119) + (-1 * x170 * (x968 + (x963 * x152)));
	const FLT x970 = (-1 * x963 * x108) + (x968 * x185);
	const FLT x971 = ((x964 * x188) + (-1 * x967 * x187)) * x189;
	const FLT x972 = (-1 * x971) + (x970 * x184);
	const FLT x973 = x969 * x175;
	const FLT x974 = (x969 * x177) + (x120 * (x973 + (-1 * x969 * x174)));
	const FLT x975 = (x969 * x179) + (x974 * x120);
	const FLT x976 = x971 + (-1 * x141 * (x970 + (-1 * x972 * x208) + (x975 * x181) + (x969 * x207) + (-1 * x204 * ((x202 * ((x969 * x201) + (x975 * x120) + (x120 * (x975 + (x969 * x195) + (x120 * (x974 + (x120 * ((-1 * x969 * x198) + x973 + (x969 * x196))) + (x969 * x199))))) + (x969 * x200))) + (-1 * x972 * x194)))));
	const FLT x977 = x965 + x966;
	const FLT x978 = (-1 * x947) + x948;
	const FLT x979 = x962 + x944;
	const FLT x980 = (x979 * x167) + (x978 * x158);
	const FLT x981 = (x977 * x119) + (-1 * x170 * (x980 + (x977 * x152)));
	const FLT x982 = (-1 * x977 * x108) + (x980 * x185);
	const FLT x983 = ((x978 * x188) + (-1 * x979 * x187)) * x189;
	const FLT x984 = (-1 * x983) + (x982 * x184);
	const FLT x985 = x981 * x175;
	const FLT x986 = (x981 * x177) + (x120 * (x985 + (-1 * x981 * x174)));
	const FLT x987 = (x981 * x179) + (x986 * x120);
	const FLT x988 = x983 + (-1 * x141 * (x982 + (-1 * x984 * x208) + (x987 * x181) + (x981 * x207) + (-1 * x204 * ((x202 * ((x987 * x120) + (x981 * x200) + (x120 * (x987 + (x981 * x195) + (x120 * ((x120 * ((-1 * x981 * x198) + x985 + (x981 * x196))) + x986 + (x981 * x199))))) + (x981 * x201))) + (-1 * x984 * x194)))));
	const FLT x989 = x203 * x135;
	const FLT x990 = ((-1 * x989) + x139) * x141;
	const FLT x991 = ((x989 * x111) + (-1 * x111 * x139)) * x141;
	const FLT x992 = x141 * ((-1 * x989 * x192) + x208);
	const FLT x993 = x102 * x105 * (1 + x183);
	const FLT x994 = x993 * x184;
	const FLT x995 = x116 * x172;
	const FLT x996 = x995 * x102 * x127;
	const FLT x997 = -1 * x996 * x175;
	const FLT x998 = (-1 * x996 * x177) + (x120 * (x997 + (x996 * x174)));
	const FLT x999 = (-1 * x996 * x179) + (x998 * x120);
	const FLT x1000 = x141 * (x993 + (-1 * x994 * x208) + (-1 * x995 * x202 * x205 * x152) + (x999 * x181) + (-1 * x204 * ((x202 * ((x999 * x120) + (-1 * x996 * x200) + (-1 * x996 * x201) + (x120 * (x999 + (-1 * x996 * x195) + (x120 * (x998 + (x120 * ((x996 * x198) + x997 + (-1 * x996 * x196))) + (-1 * x996 * x199))))))) + (-1 * x117 * x134) + x127 + (-1 * x994 * x194))));
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[0])/sizeof(FLT), x209 + (x211 * x209));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[1])/sizeof(FLT), x228 + (x211 * x228));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Pos[2])/sizeof(FLT), x241 + (x211 * x241));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[0])/sizeof(FLT), x351 + (x211 * x351));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[1])/sizeof(FLT), x409 + (x409 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[2])/sizeof(FLT), x450 + (x450 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Lighthouse.Rot[3])/sizeof(FLT), x483 + (x483 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[0])/sizeof(FLT), x505 + (x505 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[1])/sizeof(FLT), x520 + (x520 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Acc[2])/sizeof(FLT), x532 + (x532 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[0])/sizeof(FLT), x546 + (x546 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[1])/sizeof(FLT), x559 + (x559 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Pos[2])/sizeof(FLT), x571 + (x571 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[0])/sizeof(FLT), x648 + (x648 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[1])/sizeof(FLT), x687 + (x687 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[2])/sizeof(FLT), x724 + (x724 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Pose.Rot[3])/sizeof(FLT), x756 + (x756 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x852 + (x852 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x904 + (x904 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x943 + (x943 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[0])/sizeof(FLT), x961 + (x961 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[1])/sizeof(FLT), x976 + (x976 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, Object.Velocity.Pos[2])/sizeof(FLT), x988 + (x988 * x211));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.curve)/sizeof(FLT), (-1 * x990 * x211) + (-1 * x990));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.gibmag)/sizeof(FLT), sin(x210));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.gibpha)/sizeof(FLT), x211);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.ogeemag)/sizeof(FLT), (-1 * x991 * x211) + (-1 * x991));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.ogeephase)/sizeof(FLT), (-1 * x992 * x211) + (-1 * x992));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.phase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanModel, BSD1.tilt)/sizeof(FLT), (-1 * x211 * x1000) + (-1 * x1000));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [(*_x0).Lighthouse.Pos[0], (*_x0).Lighthouse.Pos[1], (*_x0).Lighthouse.Pos[2], (*_x0).Lighthouse.Rot[0], (*_x0).Lighthouse.Rot[1], (*_x0).Lighthouse.Rot[2], (*_x0).Lighthouse.Rot[3], (*_x0).Object.Acc[0], (*_x0).Object.Acc[1], (*_x0).Object.Acc[2], (*_x0).Object.IMUBias.AccBias[0], (*_x0).Object.IMUBias.AccBias[1], (*_x0).Object.IMUBias.AccBias[2], (*_x0).Object.IMUBias.AccScale[0], (*_x0).Object.IMUBias.AccScale[1], (*_x0).Object.IMUBias.AccScale[2], (*_x0).Object.IMUBias.GyroBias[0], (*_x0).Object.IMUBias.GyroBias[1], (*_x0).Object.IMUBias.GyroBias[2], (*_x0).Object.IMUBias.IMUCorrection[0], (*_x0).Object.IMUBias.IMUCorrection[1], (*_x0).Object.IMUBias.IMUCorrection[2], (*_x0).Object.IMUBias.IMUCorrection[3], (*_x0).Object.Pose.Pos[0], (*_x0).Object.Pose.Pos[1], (*_x0).Object.Pose.Pos[2], (*_x0).Object.Pose.Rot[0], (*_x0).Object.Pose.Rot[1], (*_x0).Object.Pose.Rot[2], (*_x0).Object.Pose.Rot[3], (*_x0).Object.Velocity.AxisAngleRot[0], (*_x0).Object.Velocity.AxisAngleRot[1], (*_x0).Object.Velocity.AxisAngleRot[2], (*_x0).Object.Velocity.Pos[0], (*_x0).Object.Velocity.Pos[1], (*_x0).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]

static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_x0_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_x0(Hx, dt, _x0, error_model, sensor_pt);
    }
}
// Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]
static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_error_model(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x1 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x2 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x3 = (x2 * (*_x0).Lighthouse.Rot[1]) + (x1 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[3] + (-1 * x0 * (*_x0).Lighthouse.Rot[2]);
	const FLT x4 = 1.0/2.0 * x3;
	const FLT x5 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x6 = 0.5 * (*_x0).Lighthouse.Rot[3];
	const FLT x7 = (-1 * x0 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x6 * (*error_model).Lighthouse.AxisAngleRot[2]) + (-1 * x2 * (*_x0).Lighthouse.Rot[2]);
	const FLT x8 = x7 * x7;
	const FLT x9 = (*_x0).Lighthouse.Rot[1] + (x0 * (*_x0).Lighthouse.Rot[0]) + (-1 * x6 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x1 * (*_x0).Lighthouse.Rot[2]);
	const FLT x10 = 1.0 * x9;
	const FLT x11 = x10 * (*_x0).Lighthouse.Rot[0];
	const FLT x12 = (-1 * x1 * (*_x0).Lighthouse.Rot[1]) + (x6 * (*error_model).Lighthouse.AxisAngleRot[0]) + (x2 * (*_x0).Lighthouse.Rot[0]) + (*_x0).Lighthouse.Rot[2];
	const FLT x13 = 1.0 * x12;
	const FLT x14 = x13 * (*_x0).Lighthouse.Rot[3];
	const FLT x15 = 1.0 * x3;
	const FLT x16 = x15 * (*_x0).Lighthouse.Rot[2];
	const FLT x17 = 1.0 * x7;
	const FLT x18 = x17 * (*_x0).Lighthouse.Rot[1];
	const FLT x19 = (-1 * x18) + (-1 * x16) + x11 + x14;
	const FLT x20 = x3 * x3;
	const FLT x21 = x12 * x12;
	const FLT x22 = x9 * x9;
	const FLT x23 = x22 + x21 + x20 + x8;
	const FLT x24 = 1. / (x23 * x23);
	const FLT x25 = x24 * x19;
	const FLT x26 = 1. / x23;
	const FLT x27 = 1. / sqrt(x23);
	const FLT x28 = x20 * x26;
	const FLT x29 = x21 * x26;
	const FLT x30 = x22 * x26;
	const FLT x31 = x30 + (x8 * x26) + x28 + x29;
	const FLT x32 = (1. / (x31 * sqrt(x31))) * x27;
	const FLT x33 = x32 * ((-1 * x26 * x18) + (-1 * x26 * x16) + (x26 * x11) + (-1 * x8 * x25) + (-1 * x25 * x20) + (-1 * x25 * x21) + (x26 * x14) + (-1 * x25 * x22));
	const FLT x34 = x5 * x33;
	const FLT x35 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x36 = 1. / sqrt(x31);
	const FLT x37 = x36 * (1. / (x23 * sqrt(x23)));
	const FLT x38 = x37 * x19;
	const FLT x39 = 1.0/2.0 * x7;
	const FLT x40 = x38 * x39;
	const FLT x41 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x42 = x33 * x12;
	const FLT x43 = 1.0/2.0 * x42;
	const FLT x44 = x5 * x38;
	const FLT x45 = x38 * x12;
	const FLT x46 = 1.0/2.0 * x45;
	const FLT x47 = x33 * x39;
	const FLT x48 = x36 * x27;
	const FLT x49 = x6 * x48;
	const FLT x50 = 0.5 * x48;
	const FLT x51 = x50 * (*_x0).Lighthouse.Rot[2];
	const FLT x52 = x48 * x35;
	const FLT x53 = 0.5 * x52;
	const FLT x54 = (-1 * x53 * (*_x0).Lighthouse.Rot[1]) + (-1 * x41 * x49) + (-1 * x5 * x51);
	const FLT x55 = (-1 * x47 * x35) + (x41 * x43) + (-1 * x4 * x34) + x54 + (-1 * x4 * x44) + (-1 * x40 * x35) + (x41 * x46);
	const FLT x56 = x3 * x48;
	const FLT x57 = 2 * x56;
	const FLT x58 = x6 * x52;
	const FLT x59 = 1.0/2.0 * x9;
	const FLT x60 = x50 * (*_x0).Lighthouse.Rot[1];
	const FLT x61 = x60 * x41;
	const FLT x62 = x50 * (*_x0).Lighthouse.Rot[0];
	const FLT x63 = x5 * x62;
	const FLT x64 = (-1 * x63) + (x59 * x34) + (-1 * x46 * x35) + (-1 * x61) + (-1 * x43 * x35) + x58 + (-1 * x41 * x47) + (x59 * x44) + (-1 * x40 * x41);
	const FLT x65 = x9 * x48;
	const FLT x66 = 2 * x65;
	const FLT x67 = x48 * x12;
	const FLT x68 = x7 * x48;
	const FLT x69 = (-1 * x67 * x41) + (x5 * x56) + (x68 * x35);
	const FLT x70 = x3 * x69;
	const FLT x71 = dt * fabs(dt);
	const FLT x72 = 1.0/2.0 * x71;
	const FLT x73 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x74 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x75 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x76 = 0.5 * (*error_model).Object.Pose.AxisAngleRot[0];
	const FLT x77 = (*_x0).Object.Pose.Rot[0] + (-1 * x76 * (*_x0).Object.Pose.Rot[1]) + (-1 * x74 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x75 * (*error_model).Object.Pose.AxisAngleRot[1]);
	const FLT x78 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x79 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x80 = (x79 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[3] + (-1 * x76 * (*_x0).Object.Pose.Rot[2]) + (x78 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x81 = (x76 * (*_x0).Object.Pose.Rot[3]) + (-1 * x79 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x78 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2];
	const FLT x82 = (*_x0).Object.Pose.Rot[1] + (x76 * (*_x0).Object.Pose.Rot[0]) + (-1 * x74 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x75 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x83 = (x77 * x77) + (x82 * x82) + (x80 * x80) + (x81 * x81);
	const FLT x84 = 1. / sqrt(x83);
	const FLT x85 = dt * dt;
	const FLT x86 = x73 * x73;
	const FLT x87 = x85 * x86;
	const FLT x88 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x89 = x88 * x88;
	const FLT x90 = x89 * x85;
	const FLT x91 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x92 = x91 * x91;
	const FLT x93 = x85 * x92;
	const FLT x94 = 1e-10 + x93 + x87 + x90;
	const FLT x95 = sqrt(x94);
	const FLT x96 = 0.5 * x95;
	const FLT x97 = sin(x96);
	const FLT x98 = x97 * x97;
	const FLT x99 = 1. / x94;
	const FLT x100 = x99 * x98;
	const FLT x101 = cos(x96);
	const FLT x102 = (x90 * x100) + (x101 * x101) + (x87 * x100) + (x93 * x100);
	const FLT x103 = 1. / sqrt(x102);
	const FLT x104 = x97 * (1. / x95);
	const FLT x105 = dt * x104;
	const FLT x106 = x103 * x105;
	const FLT x107 = x84 * x106;
	const FLT x108 = x77 * x107;
	const FLT x109 = x82 * x84;
	const FLT x110 = x103 * x109;
	const FLT x111 = x91 * x104;
	const FLT x112 = dt * x111;
	const FLT x113 = x101 * x103;
	const FLT x114 = x80 * x84;
	const FLT x115 = x113 * x114;
	const FLT x116 = x81 * x107;
	const FLT x117 = (x88 * x116) + x115 + (x73 * x108) + (-1 * x110 * x112);
	const FLT x118 = x106 * x114;
	const FLT x119 = x84 * x103;
	const FLT x120 = x81 * x119;
	const FLT x121 = x84 * x113;
	const FLT x122 = x77 * x121;
	const FLT x123 = x105 * x110;
	const FLT x124 = (-1 * x88 * x123) + (-1 * x73 * x118) + x122 + (-1 * x112 * x120);
	const FLT x125 = x103 * x114;
	const FLT x126 = x82 * x121;
	const FLT x127 = (x88 * x108) + x126 + (-1 * x73 * x116) + (x112 * x125);
	const FLT x128 = (-1 * x127 * sensor_pt[2]) + (x124 * sensor_pt[1]) + (x117 * sensor_pt[0]);
	const FLT x129 = x77 * x119;
	const FLT x130 = x81 * x121;
	const FLT x131 = (-1 * x88 * x118) + (x73 * x123) + (x112 * x129) + x130;
	const FLT x132 = (-1 * x131 * sensor_pt[0]) + (x124 * sensor_pt[2]) + (x127 * sensor_pt[1]);
	const FLT x133 = (*_x0).Object.Pose.Pos[0] + (x72 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + sensor_pt[0] + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + (2 * ((x131 * x132) + (-1 * x117 * x128)));
	const FLT x134 = x49 * x133;
	const FLT x135 = (-1 * x117 * sensor_pt[1]) + (x124 * sensor_pt[0]) + (x131 * sensor_pt[2]);
	const FLT x136 = (*_x0).Object.Pose.Pos[1] + (x72 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + sensor_pt[1] + (2 * ((x117 * x135) + (-1 * x127 * x132)));
	const FLT x137 = x59 * x38;
	const FLT x138 = (*_x0).Object.Pose.Pos[2] + (2 * ((x128 * x127) + (-1 * x131 * x135))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x72 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x139 = x48 * x138;
	const FLT x140 = 0.5 * x139;
	const FLT x141 = x140 * (*_x0).Lighthouse.Rot[1];
	const FLT x142 = x62 * x136;
	const FLT x143 = x59 * x33;
	const FLT x144 = (-1 * x47 * x138) + x134 + (-1 * x43 * x133) + (-1 * x46 * x133) + (-1 * x142) + (x137 * x136) + (-1 * x141) + (x136 * x143) + (-1 * x40 * x138);
	const FLT x145 = (x67 * x133) + (x68 * x138) + (-1 * x65 * x136);
	const FLT x146 = 1.0 * x48;
	const FLT x147 = x146 * x145;
	const FLT x148 = x147 * (*_x0).Lighthouse.Rot[0];
	const FLT x149 = (x56 * x136) + (x68 * x133) + (-1 * x67 * x138);
	const FLT x150 = x3 * x149;
	const FLT x151 = x146 * x149;
	const FLT x152 = x69 * x146;
	const FLT x153 = x9 * x145;
	const FLT x154 = (x67 * x35) + (x68 * x41) + (-1 * x5 * x65);
	const FLT x155 = x9 * x154;
	const FLT x156 = x146 * x154;
	const FLT x157 = x156 * (*_x0).Lighthouse.Rot[0];
	const FLT x158 = x4 * x136;
	const FLT x159 = x33 * x133;
	const FLT x160 = (-1 * x6 * x139) + (-1 * x51 * x136) + (-1 * x60 * x133);
	const FLT x161 = x160 + (x43 * x138) + (-1 * x40 * x133) + (-1 * x38 * x158) + (-1 * x33 * x158) + (x46 * x138) + (-1 * x39 * x159);
	const FLT x162 = (x38 * x155) + (-1 * x57 * x161) + (-1 * x33 * x153) + (x38 * x150) + (x57 * x55) + (x66 * x144) + x148 + (-1 * x70 * x33) + (x151 * (*_x0).Lighthouse.Rot[2]) + (x33 * x155) + (-1 * x64 * x66) + (x33 * x150) + (-1 * x152 * (*_x0).Lighthouse.Rot[2]) + (-1 * x157) + (-1 * x70 * x38) + (-1 * x38 * x153);
	const FLT x163 = x136 + (2 * ((-1 * x56 * x149) + (x65 * x145))) + (-1 * (x5 + (2 * ((-1 * x69 * x56) + (x65 * x154)))));
	const FLT x164 = 2 * x163;
	const FLT x165 = x12 * x145;
	const FLT x166 = 2 * x67;
	const FLT x167 = (x68 * x136) + (x65 * x138) + (-1 * x56 * x133);
	const FLT x168 = x38 * x167;
	const FLT x169 = (x5 * x68) + (x65 * x41) + (-1 * x56 * x35);
	const FLT x170 = x3 * x169;
	const FLT x171 = x140 * (*_x0).Lighthouse.Rot[0];
	const FLT x172 = x4 * x133;
	const FLT x173 = x60 * x136;
	const FLT x174 = x51 * x133;
	const FLT x175 = (x4 * x159) + (-1 * x47 * x136) + (x38 * x172) + (-1 * x173) + (-1 * x40 * x136) + x171 + (-1 * x138 * x143) + x174 + (-1 * x137 * x138);
	const FLT x176 = x59 * x41;
	const FLT x177 = x62 * x41;
	const FLT x178 = x4 * x35;
	const FLT x179 = x5 * x60;
	const FLT x180 = x53 * (*_x0).Lighthouse.Rot[2];
	const FLT x181 = x180 + (-1 * x5 * x40) + (x33 * x178) + (-1 * x179) + (-1 * x34 * x39) + (-1 * x38 * x176) + x177 + (x38 * x178) + (-1 * x33 * x176);
	const FLT x182 = x33 * x167;
	const FLT x183 = x167 * x146;
	const FLT x184 = x146 * (*_x0).Lighthouse.Rot[2];
	const FLT x185 = (x169 * x184) + (-1 * x183 * (*_x0).Lighthouse.Rot[2]);
	const FLT x186 = (x156 * (*_x0).Lighthouse.Rot[3]) + (-1 * x147 * (*_x0).Lighthouse.Rot[3]);
	const FLT x187 = x186 + x185 + (-1 * x3 * x182) + (-1 * x45 * x154) + (-1 * x57 * x181) + (-1 * x3 * x168) + (x38 * x170) + (x33 * x165) + (-1 * x166 * x144) + (-1 * x42 * x154) + (x45 * x145) + (x57 * x175) + (x64 * x166) + (x33 * x170);
	const FLT x188 = x133 + (-1 * (x35 + (2 * ((-1 * x67 * x154) + (x56 * x169))))) + (2 * ((-1 * x67 * x145) + (x56 * x167)));
	const FLT x189 = 2 * x188;
	const FLT x190 = x12 * x149;
	const FLT x191 = x9 * x169;
	const FLT x192 = x183 * (*_x0).Lighthouse.Rot[0];
	const FLT x193 = x169 * x146;
	const FLT x194 = x193 * (*_x0).Lighthouse.Rot[0];
	const FLT x195 = (-1 * x66 * x175) + (-1 * x152 * (*_x0).Lighthouse.Rot[3]) + x194 + (-1 * x33 * x190) + (-1 * x38 * x191) + (-1 * x55 * x166) + (x69 * x45) + (x66 * x181) + (x9 * x168) + (-1 * x38 * x190) + (x161 * x166) + (x151 * (*_x0).Lighthouse.Rot[3]) + (x9 * x182) + (x69 * x42) + (-1 * x33 * x191) + (-1 * x192);
	const FLT x196 = (-1 * (x41 + (2 * ((-1 * x65 * x169) + (x67 * x69))))) + x138 + (2 * ((-1 * x65 * x167) + (x67 * x149)));
	const FLT x197 = 2 * x196;
	const FLT x198 = (x197 * x195) + (x187 * x189);
	const FLT x199 = 0.523598775598299 + (-1 * (*error_model).BSD1.tilt) + (-1 * (*_x0).BSD1.tilt);
	const FLT x200 = cos(x199);
	const FLT x201 = 1. / x200;
	const FLT x202 = x163 * x163;
	const FLT x203 = x188 * x188;
	const FLT x204 = x203 + (x196 * x196);
	const FLT x205 = x204 + x202;
	const FLT x206 = 1.0/2.0 * x163;
	const FLT x207 = x201 * (1. / (x205 * sqrt(x205))) * x206;
	const FLT x208 = 1. / sqrt(x205);
	const FLT x209 = x201 * x208;
	const FLT x210 = (x209 * x162) + (-1 * x207 * (x198 + (x164 * x162)));
	const FLT x211 = 1. / (x200 * x200);
	const FLT x212 = 1. / sqrt(1 + (-1 * x211 * x202 * (1. / x205)));
	const FLT x213 = asin(x209 * x163);
	const FLT x214 = 8.0108022e-06 * x213;
	const FLT x215 = x212 * x214;
	const FLT x216 = -8.0108022e-06 + (-1 * x214);
	const FLT x217 = x212 * x216;
	const FLT x218 = x210 * x217;
	const FLT x219 = 0.0028679863 + (x213 * x216);
	const FLT x220 = x212 * x219;
	const FLT x221 = (x210 * x220) + (x213 * (x218 + (-1 * x210 * x215)));
	const FLT x222 = 5.3685255e-06 + (x213 * x219);
	const FLT x223 = x212 * x222;
	const FLT x224 = (x210 * x223) + (x213 * x221);
	const FLT x225 = atan2(-1 * x196, x188);
	const FLT x226 = 1. / sqrt(x204);
	const FLT x227 = tan(x199);
	const FLT x228 = x227 * x226;
	const FLT x229 = -1 * x228 * x163;
	const FLT x230 = asin(x229) + (-1 * (*_x0).BSD1.ogeephase) + (-1 * (*error_model).BSD1.ogeephase) + (-1 * x225);
	const FLT x231 = sin(x230);
	const FLT x232 = (*error_model).BSD1.ogeemag + (*_x0).BSD1.ogeemag;
	const FLT x233 = (*_x0).BSD1.curve + (-1 * x232 * x231) + (*error_model).BSD1.curve;
	const FLT x234 = x213 * x213;
	const FLT x235 = 0.0076069798 + (x213 * x222);
	const FLT x236 = x213 * x235;
	const FLT x237 = -8.0108022e-06 + (-1.60216044e-05 * x213);
	const FLT x238 = x219 + (x213 * x237);
	const FLT x239 = x222 + (x213 * x238);
	const FLT x240 = x235 + (x213 * x239);
	const FLT x241 = (x213 * x240) + x236;
	const FLT x242 = sin(x199);
	const FLT x243 = x233 * x242;
	const FLT x244 = x241 * x243;
	const FLT x245 = x200 + x244;
	const FLT x246 = 1. / x245;
	const FLT x247 = x234 * x246;
	const FLT x248 = x233 * x247;
	const FLT x249 = x212 * x236 * x246;
	const FLT x250 = 2 * x233;
	const FLT x251 = x250 * x249;
	const FLT x252 = 1. / x204;
	const FLT x253 = x227 * x227;
	const FLT x254 = 1. / sqrt(1 + (-1 * x202 * x252 * x253));
	const FLT x255 = (1. / (x204 * sqrt(x204))) * x206 * x227;
	const FLT x256 = (-1 * x228 * x162) + (x255 * x198);
	const FLT x257 = 1. / x188;
	const FLT x258 = (1. / x203) * x196;
	const FLT x259 = x203 * x252;
	const FLT x260 = ((x258 * x187) + (-1 * x257 * x195)) * x259;
	const FLT x261 = (-1 * x260) + (x254 * x256);
	const FLT x262 = x232 * cos(x230);
	const FLT x263 = x241 * x242;
	const FLT x264 = x262 * x263;
	const FLT x265 = x212 * x240;
	const FLT x266 = x212 * x239;
	const FLT x267 = x212 * x237;
	const FLT x268 = 2.40324066e-05 * x213;
	const FLT x269 = x212 * x268;
	const FLT x270 = x212 * x238;
	const FLT x271 = x212 * x235;
	const FLT x272 = x234 * x235 * (1. / (x245 * x245));
	const FLT x273 = x233 * x272;
	const FLT x274 = x235 * x247;
	const FLT x275 = x274 * x262;
	const FLT x276 = x229 + (x233 * x274);
	const FLT x277 = 1. / sqrt(1 + (-1 * (x276 * x276)));
	const FLT x278 = x260 + (-1 * x277 * (x256 + (-1 * x275 * x261) + (x224 * x248) + (-1 * x273 * ((x243 * ((x213 * x224) + (x210 * x265) + (x210 * x271) + (x213 * (x224 + (x210 * x266) + (x213 * (x221 + (x213 * ((-1 * x210 * x269) + x218 + (x210 * x267))) + (x210 * x270))))))) + (-1 * x264 * x261))) + (x210 * x251)));
	const FLT x279 = (-1 * asin(x276)) + x225 + (*error_model).BSD1.gibpha + (*_x0).BSD1.gibpha;
	const FLT x280 = cos(x279) * ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag);
	const FLT x281 = x10 * (*_x0).Lighthouse.Rot[3];
	const FLT x282 = x13 * (*_x0).Lighthouse.Rot[0];
	const FLT x283 = x15 * (*_x0).Lighthouse.Rot[1];
	const FLT x284 = x17 * (*_x0).Lighthouse.Rot[2];
	const FLT x285 = (-1 * x284) + x283 + (-1 * x281) + x282;
	const FLT x286 = x37 * x285;
	const FLT x287 = x24 * x285;
	const FLT x288 = x32 * ((x26 * x282) + (x26 * x283) + (-1 * x26 * x284) + (-1 * x22 * x287) + (-1 * x26 * x281) + (-1 * x20 * x287) + (-1 * x21 * x287) + (-1 * x8 * x287));
	const FLT x289 = x4 * x286;
	const FLT x290 = 1.0/2.0 * x12;
	const FLT x291 = x290 * x286;
	const FLT x292 = x39 * x286;
	const FLT x293 = x288 * x138;
	const FLT x294 = x288 * x133;
	const FLT x295 = 2 * ((-1 * x39 * x294) + (-1 * x174) + (-1 * x289 * x136) + (x293 * x290) + x173 + (x291 * x138) + (-1 * x171) + (-1 * x288 * x158) + (-1 * x292 * x133));
	const FLT x296 = x286 * x133;
	const FLT x297 = x62 * x133;
	const FLT x298 = x59 * x286;
	const FLT x299 = x140 * (*_x0).Lighthouse.Rot[2];
	const FLT x300 = x49 * x136;
	const FLT x301 = (x59 * x288 * x136) + x300 + (-1 * x299) + (-1 * x294 * x290) + x297 + (-1 * x296 * x290) + (x298 * x136) + (-1 * x292 * x138) + (-1 * x39 * x293);
	const FLT x302 = x290 * x288;
	const FLT x303 = x41 * x39;
	const FLT x304 = x51 * x41;
	const FLT x305 = x5 * x49;
	const FLT x306 = x53 * (*_x0).Lighthouse.Rot[0];
	const FLT x307 = x5 * x288;
	const FLT x308 = x306 + x305 + (-1 * x41 * x292) + (-1 * x35 * x302) + (x5 * x298) + (x59 * x307) + (-1 * x35 * x291) + (-1 * x288 * x303) + (-1 * x304);
	const FLT x309 = x39 * x288;
	const FLT x310 = (-1 * x35 * x309) + (-1 * x180) + x179 + (-1 * x5 * x289) + (-1 * x177) + (x41 * x291) + (-1 * x35 * x292) + (x41 * x302) + (-1 * x4 * x307);
	const FLT x311 = (x152 * (*_x0).Lighthouse.Rot[1]) + (-1 * x151 * (*_x0).Lighthouse.Rot[1]);
	const FLT x312 = x311 + (x288 * x155) + (-1 * x286 * x153) + (-1 * x66 * x308) + (-1 * x56 * x295) + (-1 * x70 * x286) + (-1 * x70 * x288) + x186 + (x286 * x155) + (x286 * x150) + (x288 * x150) + (x66 * x301) + (-1 * x288 * x153) + (x57 * x310);
	const FLT x313 = x160 + (x288 * x172) + (-1 * x309 * x136) + (-1 * x292 * x136) + (-1 * x298 * x138) + (x4 * x296) + (-1 * x59 * x293);
	const FLT x314 = x288 * x167;
	const FLT x315 = x12 * x154;
	const FLT x316 = x286 * x167;
	const FLT x317 = x54 + (x288 * x178) + (x286 * x178) + (-1 * x288 * x176) + (-1 * x5 * x292) + (-1 * x41 * x298) + (-1 * x39 * x307);
	const FLT x318 = (x286 * x165) + (x183 * (*_x0).Lighthouse.Rot[1]) + (x288 * x165) + (-1 * x288 * x315) + (x288 * x170) + (-1 * x193 * (*_x0).Lighthouse.Rot[1]) + (x308 * x166) + (x286 * x170) + (-1 * x57 * x317) + (-1 * x3 * x314) + (-1 * x301 * x166) + x157 + (x57 * x313) + (-1 * x3 * x316) + (-1 * x286 * x315) + (-1 * x148);
	const FLT x319 = x151 * (*_x0).Lighthouse.Rot[0];
	const FLT x320 = x69 * x12;
	const FLT x321 = x152 * (*_x0).Lighthouse.Rot[0];
	const FLT x322 = (x9 * x314) + (x9 * x316) + (-1 * x310 * x166) + (x288 * x320) + (-1 * x286 * x191) + (-1 * x193 * (*_x0).Lighthouse.Rot[3]) + (-1 * x66 * x313) + (x67 * x295) + (-1 * x288 * x190) + (-1 * x286 * x190) + (x183 * (*_x0).Lighthouse.Rot[3]) + (-1 * x288 * x191) + x319 + (x286 * x320) + (-1 * x321) + (x66 * x317);
	const FLT x323 = (x322 * x197) + (x318 * x189);
	const FLT x324 = (x209 * x312) + (-1 * x207 * (x323 + (x312 * x164)));
	const FLT x325 = (-1 * x228 * x312) + (x255 * x323);
	const FLT x326 = ((x258 * x318) + (-1 * x257 * x322)) * x259;
	const FLT x327 = (-1 * x326) + (x254 * x325);
	const FLT x328 = x217 * x324;
	const FLT x329 = (x220 * x324) + (x213 * (x328 + (-1 * x215 * x324)));
	const FLT x330 = (x223 * x324) + (x213 * x329);
	const FLT x331 = x326 + (-1 * x277 * ((x248 * x330) + x325 + (x251 * x324) + (-1 * x275 * x327) + (-1 * x273 * ((x243 * ((x213 * x330) + (x271 * x324) + (x265 * x324) + (x213 * (x330 + (x266 * x324) + (x213 * (x329 + (x213 * ((-1 * x269 * x324) + x328 + (x267 * x324))) + (x270 * x324))))))) + (-1 * x264 * x327)))));
	const FLT x332 = x10 * (*_x0).Lighthouse.Rot[2];
	const FLT x333 = x13 * (*_x0).Lighthouse.Rot[1];
	const FLT x334 = x15 * (*_x0).Lighthouse.Rot[0];
	const FLT x335 = x17 * (*_x0).Lighthouse.Rot[3];
	const FLT x336 = (-1 * x335) + x334 + x332 + (-1 * x333);
	const FLT x337 = x24 * x336;
	const FLT x338 = x32 * ((-1 * x26 * x335) + (-1 * x8 * x337) + (x26 * x332) + (-1 * x22 * x337) + (-1 * x20 * x337) + (-1 * x26 * x333) + (-1 * x21 * x337) + (x26 * x334));
	const FLT x339 = x290 * x338;
	const FLT x340 = x37 * x336;
	const FLT x341 = x39 * x340;
	const FLT x342 = x290 * x340;
	const FLT x343 = x5 * x340;
	const FLT x344 = x5 * x338;
	const FLT x345 = x54 + (-1 * x41 * x341) + (-1 * x35 * x339) + (x59 * x343) + (-1 * x35 * x342) + (x59 * x344) + (-1 * x303 * x338);
	const FLT x346 = (-1 * x35 * x341) + (-1 * x4 * x344) + (x41 * x339) + (x41 * x342) + x63 + (-1 * x58) + x61 + (-1 * x35 * x39 * x338) + (-1 * x4 * x343);
	const FLT x347 = x338 * x133;
	const FLT x348 = x59 * x340;
	const FLT x349 = x338 * x138;
	const FLT x350 = x338 * x136;
	const FLT x351 = x160 + (x348 * x136) + (-1 * x290 * x347) + (x59 * x350) + (-1 * x341 * x138) + (-1 * x39 * x349) + (-1 * x342 * x133);
	const FLT x352 = (x290 * x349) + (-1 * x134) + (-1 * x341 * x133) + x142 + (-1 * x39 * x347) + x141 + (x342 * x138) + (-1 * x4 * x350) + (-1 * x340 * x158);
	const FLT x353 = (x338 * x150) + (-1 * x57 * x352) + (x340 * x150) + (x147 * (*_x0).Lighthouse.Rot[2]) + (x57 * x346) + x321 + (-1 * x184 * x154) + (-1 * x340 * x153) + (-1 * x66 * x345) + (-1 * x338 * x153) + (x338 * x155) + (x66 * x351) + (-1 * x70 * x338) + (x340 * x155) + (-1 * x70 * x340) + (-1 * x319);
	const FLT x354 = x340 * x167;
	const FLT x355 = (-1 * x297) + (-1 * x341 * x136) + (-1 * x300) + (-1 * x59 * x349) + (-1 * x39 * x350) + (x340 * x172) + (x338 * x172) + (-1 * x348 * x138) + x299;
	const FLT x356 = (x338 * x178) + (-1 * x5 * x341) + (-1 * x338 * x176) + (-1 * x39 * x344) + (-1 * x305) + (-1 * x306) + (-1 * x340 * x176) + x304 + (x340 * x178);
	const FLT x357 = x338 * x167;
	const FLT x358 = (x147 * (*_x0).Lighthouse.Rot[1]) + (x340 * x165) + (-1 * x351 * x166) + (x345 * x166) + (-1 * x3 * x354) + (-1 * x194) + (x338 * x165) + (x338 * x170) + x192 + (x340 * x170) + (-1 * x57 * x356) + (x57 * x355) + (-1 * x156 * (*_x0).Lighthouse.Rot[1]) + (-1 * x3 * x357) + (-1 * x315 * x340) + (-1 * x338 * x315);
	const FLT x359 = x185 + (-1 * x66 * x355) + (x352 * x166) + (-1 * x340 * x191) + (-1 * x346 * x166) + (x9 * x354) + (x9 * x357) + (-1 * x338 * x190) + (x338 * x320) + (x320 * x340) + x311 + (x66 * x356) + (-1 * x340 * x190) + (-1 * x338 * x191);
	const FLT x360 = (x359 * x197) + (x358 * x189);
	const FLT x361 = (x209 * x353) + (-1 * x207 * (x360 + (x353 * x164)));
	const FLT x362 = x217 * x361;
	const FLT x363 = (x220 * x361) + (x213 * (x362 + (-1 * x215 * x361)));
	const FLT x364 = (x223 * x361) + (x213 * x363);
	const FLT x365 = (-1 * x228 * x353) + (x255 * x360);
	const FLT x366 = ((x258 * x358) + (-1 * x257 * x359)) * x259;
	const FLT x367 = (-1 * x366) + (x254 * x365);
	const FLT x368 = x366 + (-1 * x277 * (x365 + (-1 * x275 * x367) + (-1 * x273 * ((x243 * ((x271 * x361) + (x265 * x361) + (x213 * x364) + (x213 * (x364 + (x266 * x361) + (x213 * (x363 + (x213 * (x362 + (-1 * x269 * x361) + (x267 * x361))) + (x270 * x361))))))) + (-1 * x264 * x367))) + (x248 * x364) + (x251 * x361)));
	const FLT x369 = 1. / x31;
	const FLT x370 = 2 * x369;
	const FLT x371 = x26 * x370;
	const FLT x372 = x3 * x371;
	const FLT x373 = x7 * x372;
	const FLT x374 = x12 * x371;
	const FLT x375 = x9 * x374;
	const FLT x376 = -1 * x375;
	const FLT x377 = x376 + x373;
	const FLT x378 = x29 * x370;
	const FLT x379 = x28 * x370;
	const FLT x380 = -1 + x379;
	const FLT x381 = x380 + x378;
	const FLT x382 = x9 * x372;
	const FLT x383 = -1 * x382;
	const FLT x384 = x7 * x374;
	const FLT x385 = -1 * x384;
	const FLT x386 = x385 + x383;
	const FLT x387 = (x386 * x197) + (x381 * x189);
	const FLT x388 = (x209 * x377) + (-1 * x207 * (x387 + (x377 * x164)));
	const FLT x389 = x217 * x388;
	const FLT x390 = (x220 * x388) + (x213 * (x389 + (-1 * x215 * x388)));
	const FLT x391 = (x223 * x388) + (x213 * x390);
	const FLT x392 = (-1 * x228 * x377) + (x255 * x387);
	const FLT x393 = ((x258 * x381) + (-1 * x257 * x386)) * x259;
	const FLT x394 = (-1 * x393) + (x254 * x392);
	const FLT x395 = x393 + (-1 * x277 * (x392 + (-1 * x275 * x394) + (x251 * x388) + (x248 * x391) + (-1 * x273 * ((x243 * ((x213 * x391) + (x265 * x388) + (x213 * (x391 + (x266 * x388) + (x213 * ((x213 * ((-1 * x269 * x388) + x389 + (x267 * x388))) + x390 + (x270 * x388))))) + (x271 * x388))) + (-1 * x264 * x394)))));
	const FLT x396 = x30 * x370;
	const FLT x397 = x380 + x396;
	const FLT x398 = -1 * x373;
	const FLT x399 = x398 + x376;
	const FLT x400 = x7 * x9;
	const FLT x401 = x400 * x371;
	const FLT x402 = x12 * x372;
	const FLT x403 = -1 * x402;
	const FLT x404 = x403 + x401;
	const FLT x405 = (x404 * x197) + (x399 * x189);
	const FLT x406 = (x209 * x397) + (-1 * x207 * (x405 + (x397 * x164)));
	const FLT x407 = (-1 * x228 * x397) + (x405 * x255);
	const FLT x408 = ((x258 * x399) + (-1 * x404 * x257)) * x259;
	const FLT x409 = (-1 * x408) + (x407 * x254);
	const FLT x410 = x406 * x217;
	const FLT x411 = (x406 * x220) + (x213 * (x410 + (-1 * x406 * x215)));
	const FLT x412 = (x406 * x223) + (x411 * x213);
	const FLT x413 = x408 + (-1 * x277 * (x407 + (-1 * x409 * x275) + (x406 * x251) + (x412 * x248) + (-1 * x273 * ((x243 * ((x412 * x213) + (x406 * x271) + (x406 * x265) + (x213 * (x412 + (x406 * x266) + (x213 * ((x213 * ((-1 * x406 * x269) + x410 + (x406 * x267))) + x411 + (x406 * x270))))))) + (-1 * x409 * x264)))));
	const FLT x414 = x383 + x384;
	const FLT x415 = -1 + x378 + x396;
	const FLT x416 = (x415 * x197) + (x414 * x189);
	const FLT x417 = -1 * x401;
	const FLT x418 = x417 + x403;
	const FLT x419 = (-1 * x418 * x228) + (x416 * x255);
	const FLT x420 = ((x414 * x258) + (-1 * x415 * x257)) * x259;
	const FLT x421 = (-1 * x420) + (x419 * x254);
	const FLT x422 = (x418 * x209) + (-1 * x207 * (x416 + (x418 * x164)));
	const FLT x423 = x422 * x217;
	const FLT x424 = (x422 * x220) + (x213 * (x423 + (-1 * x422 * x215)));
	const FLT x425 = (x422 * x223) + (x424 * x213);
	const FLT x426 = x420 + (-1 * x277 * (x419 + (-1 * x421 * x275) + (x422 * x251) + (-1 * x273 * ((x243 * ((x425 * x213) + (x422 * x271) + (x422 * x265) + (x213 * (x425 + (x422 * x266) + (x213 * (x424 + (x213 * ((-1 * x422 * x269) + x423 + (x422 * x267))) + (x422 * x270))))))) + (-1 * x421 * x264))) + (x425 * x248)));
	const FLT x427 = x71 * x369;
	const FLT x428 = x26 * x427;
	const FLT x429 = x9 * x428;
	const FLT x430 = x12 * x429;
	const FLT x431 = x3 * x428;
	const FLT x432 = x7 * x431;
	const FLT x433 = (-1 * x432) + x430;
	const FLT x434 = -1 * x28 * x427;
	const FLT x435 = (-1 * x29 * x427) + x72;
	const FLT x436 = x435 + x434;
	const FLT x437 = x7 * x12 * x428;
	const FLT x438 = x3 * x429;
	const FLT x439 = x438 + x437;
	const FLT x440 = (x439 * x197) + (x436 * x189);
	const FLT x441 = (x433 * x209) + (-1 * x207 * (x440 + (x433 * x164)));
	const FLT x442 = (-1 * x433 * x228) + (x440 * x255);
	const FLT x443 = ((x436 * x258) + (-1 * x439 * x257)) * x259;
	const FLT x444 = (-1 * x443) + (x442 * x254);
	const FLT x445 = x441 * x217;
	const FLT x446 = (x441 * x220) + (x213 * (x445 + (-1 * x441 * x215)));
	const FLT x447 = (x441 * x223) + (x446 * x213);
	const FLT x448 = x443 + (-1 * x277 * (x442 + (-1 * x444 * x275) + (x447 * x248) + (x441 * x251) + (-1 * x273 * ((x243 * ((x447 * x213) + (x441 * x265) + (x213 * (x447 + (x441 * x266) + (x213 * (x446 + (x213 * ((-1 * x441 * x269) + x445 + (x441 * x267))) + (x441 * x270))))) + (x441 * x271))) + (-1 * x444 * x264)))));
	const FLT x449 = -1 * x30 * x427;
	const FLT x450 = x449 + x72 + x434;
	const FLT x451 = x430 + x432;
	const FLT x452 = x12 * x431;
	const FLT x453 = x400 * x428;
	const FLT x454 = (-1 * x453) + x452;
	const FLT x455 = (x454 * x197) + (x451 * x189);
	const FLT x456 = (x450 * x209) + (-1 * x207 * (x455 + (x450 * x164)));
	const FLT x457 = x456 * x217;
	const FLT x458 = (x456 * x220) + (x213 * (x457 + (-1 * x456 * x215)));
	const FLT x459 = (x456 * x223) + (x458 * x213);
	const FLT x460 = (-1 * x450 * x228) + (x455 * x255);
	const FLT x461 = ((x451 * x258) + (-1 * x454 * x257)) * x259;
	const FLT x462 = (-1 * x461) + (x460 * x254);
	const FLT x463 = x461 + (-1 * x277 * (x460 + (x459 * x248) + (-1 * x462 * x275) + (-1 * x273 * ((x243 * ((x459 * x213) + (x213 * (x459 + (x456 * x266) + (x213 * (x458 + (x213 * ((-1 * x456 * x269) + x457 + (x456 * x267))) + (x456 * x270))))) + (x456 * x271) + (x456 * x265))) + (-1 * x462 * x264))) + (x456 * x251)));
	const FLT x464 = x452 + x453;
	const FLT x465 = (-1 * x437) + x438;
	const FLT x466 = x435 + x449;
	const FLT x467 = (x466 * x197) + (x465 * x189);
	const FLT x468 = (x464 * x209) + (-1 * x207 * (x467 + (x464 * x164)));
	const FLT x469 = x468 * x217;
	const FLT x470 = (x468 * x220) + (x213 * (x469 + (-1 * x468 * x215)));
	const FLT x471 = (x468 * x223) + (x470 * x213);
	const FLT x472 = (-1 * x464 * x228) + (x467 * x255);
	const FLT x473 = ((x465 * x258) + (-1 * x466 * x257)) * x259;
	const FLT x474 = (-1 * x473) + (x472 * x254);
	const FLT x475 = x473 + (-1 * x277 * (x472 + (-1 * x474 * x275) + (-1 * x273 * ((x243 * ((x213 * (x471 + (x468 * x266) + (x213 * (x470 + (x213 * ((-1 * x468 * x269) + x469 + (x468 * x267))) + (x468 * x270))))) + (x468 * x265) + (x471 * x213) + (x468 * x271))) + (-1 * x474 * x264))) + (x471 * x248) + (x468 * x251)));
	const FLT x476 = 1.0 * x82;
	const FLT x477 = 1.0 * x77;
	const FLT x478 = 1.0 * (*_x0).Object.Pose.Rot[2];
	const FLT x479 = 1.0 * x81;
	const FLT x480 = (x479 * (*_x0).Object.Pose.Rot[3]) + (-1 * x80 * x478) + (x476 * (*_x0).Object.Pose.Rot[0]) + (-1 * x477 * (*_x0).Object.Pose.Rot[1]);
	const FLT x481 = 1. / (x83 * sqrt(x83));
	const FLT x482 = x481 * x113;
	const FLT x483 = x482 * x480;
	const FLT x484 = 1.0/2.0 * x483;
	const FLT x485 = 1.0/2.0 * x77;
	const FLT x486 = x481 * x485;
	const FLT x487 = x88 * x106;
	const FLT x488 = x487 * x480;
	const FLT x489 = x73 * x106;
	const FLT x490 = x489 * x480;
	const FLT x491 = 1.0/2.0 * x81;
	const FLT x492 = x491 * x481;
	const FLT x493 = x103 * x112;
	const FLT x494 = 1.0/2.0 * x80;
	const FLT x495 = x494 * x493;
	const FLT x496 = x481 * x480;
	const FLT x497 = x112 * x119;
	const FLT x498 = x73 * x107;
	const FLT x499 = x88 * x107;
	const FLT x500 = (-1 * x79 * x499) + (-1 * x75 * x497) + (-1 * x74 * x498) + (x78 * x121);
	const FLT x501 = x500 + (-1 * x495 * x496) + (x490 * x492) + (-1 * x82 * x484) + (-1 * x488 * x486);
	const FLT x502 = 2 * x132;
	const FLT x503 = x481 * x488;
	const FLT x504 = 1.0/2.0 * x82;
	const FLT x505 = x493 * x504;
	const FLT x506 = x74 * x499;
	const FLT x507 = x78 * x497;
	const FLT x508 = x79 * x498;
	const FLT x509 = x75 * x121;
	const FLT x510 = (-1 * x509) + (-1 * x508) + x506 + (-1 * x507);
	const FLT x511 = x510 + (x496 * x505) + (-1 * x491 * x503) + (-1 * x80 * x484) + (-1 * x490 * x486);
	const FLT x512 = x494 * x489;
	const FLT x513 = x74 * x497;
	const FLT x514 = x79 * x121;
	const FLT x515 = x78 * x499;
	const FLT x516 = x75 * x498;
	const FLT x517 = x516 + (-1 * x515) + (-1 * x513) + (-1 * x514);
	const FLT x518 = x517 + (x492 * x493 * x480) + (x496 * x512) + (x503 * x504) + (-1 * x483 * x485);
	const FLT x519 = x493 * x486;
	const FLT x520 = x75 * x499;
	const FLT x521 = x79 * x497;
	const FLT x522 = x78 * x498;
	const FLT x523 = x74 * x121;
	const FLT x524 = x522 + (-1 * x480 * x519) + (-1 * x521) + x523 + (x494 * x503) + x520 + (-1 * x491 * x483) + (-1 * x490 * x481 * x504);
	const FLT x525 = (x524 * sensor_pt[2]) + (-1 * x511 * sensor_pt[1]) + (x518 * sensor_pt[0]);
	const FLT x526 = 2 * x117;
	const FLT x527 = 2 * x135;
	const FLT x528 = 2 * ((x501 * sensor_pt[1]) + (-1 * x524 * sensor_pt[0]) + (x518 * sensor_pt[2]));
	const FLT x529 = (-1 * x528 * x127) + (x511 * x527) + (-1 * x501 * x502) + (x525 * x526);
	const FLT x530 = 2 * x131;
	const FLT x531 = 2 * x128;
	const FLT x532 = (x511 * sensor_pt[0]) + (-1 * x501 * sensor_pt[2]) + (x518 * sensor_pt[1]);
	const FLT x533 = 2 * x127;
	const FLT x534 = (x533 * x532) + (-1 * x524 * x527) + (x501 * x531) + (-1 * x525 * x530);
	const FLT x535 = (x524 * x502) + (-1 * x526 * x532) + (-1 * x511 * x531) + (x528 * x131);
	const FLT x536 = (-1 * x56 * x535) + (x68 * x529) + (x65 * x534);
	const FLT x537 = (-1 * x65 * x529) + (x67 * x535) + (x68 * x534);
	const FLT x538 = (x57 * x536) + x535 + (-1 * x537 * x166);
	const FLT x539 = (-1 * x67 * x534) + (x56 * x529) + (x68 * x535);
	const FLT x540 = (x539 * x166) + x534 + (-1 * x66 * x536);
	const FLT x541 = (x540 * x197) + (x538 * x189);
	const FLT x542 = (x66 * x537) + x529 + (-1 * x57 * x539);
	const FLT x543 = (-1 * x542 * x228) + (x541 * x255);
	const FLT x544 = ((x538 * x258) + (-1 * x540 * x257)) * x259;
	const FLT x545 = (-1 * x544) + (x543 * x254);
	const FLT x546 = (x542 * x209) + (-1 * x207 * (x541 + (x542 * x164)));
	const FLT x547 = x546 * x217;
	const FLT x548 = (x546 * x220) + (x213 * (x547 + (-1 * x546 * x215)));
	const FLT x549 = (x546 * x223) + (x548 * x213);
	const FLT x550 = x544 + (-1 * x277 * ((-1 * x545 * x275) + x543 + (x546 * x251) + (-1 * x273 * ((x243 * ((x546 * x271) + (x549 * x213) + (x546 * x265) + (x213 * (x549 + (x546 * x266) + (x213 * (x548 + (x213 * ((-1 * x546 * x269) + x547 + (x546 * x267))) + (x546 * x270))))))) + (-1 * x545 * x264))) + (x549 * x248)));
	const FLT x551 = 1.0 * x80;
	const FLT x552 = (-1 * x77 * x478) + (-1 * x476 * (*_x0).Object.Pose.Rot[3]) + (x479 * (*_x0).Object.Pose.Rot[0]) + (x551 * (*_x0).Object.Pose.Rot[1]);
	const FLT x553 = x481 * x552;
	const FLT x554 = x487 * x553;
	const FLT x555 = x489 * x553;
	const FLT x556 = x486 * x552;
	const FLT x557 = x482 * x552;
	const FLT x558 = x500 + (-1 * x491 * x557) + (-1 * x493 * x556) + (x494 * x554) + (-1 * x504 * x555);
	const FLT x559 = x491 * x553;
	const FLT x560 = 1.0/2.0 * x557;
	const FLT x561 = (-1 * x523) + x521 + (-1 * x522) + (-1 * x520);
	const FLT x562 = x561 + (-1 * x82 * x560) + (-1 * x495 * x553) + (-1 * x487 * x556) + (x489 * x559);
	const FLT x563 = x510 + (-1 * x77 * x560) + (x493 * x559) + (x504 * x554) + (x494 * x555);
	const FLT x564 = x514 + (-1 * x80 * x560) + (-1 * x491 * x554) + x513 + (x505 * x553) + (-1 * x516) + (-1 * x489 * x556) + x515;
	const FLT x565 = (x564 * sensor_pt[0]) + (-1 * x562 * sensor_pt[2]) + (x563 * sensor_pt[1]);
	const FLT x566 = (x558 * sensor_pt[2]) + (-1 * x564 * sensor_pt[1]) + (x563 * sensor_pt[0]);
	const FLT x567 = (x531 * x562) + (-1 * x527 * x558) + (-1 * x530 * x566) + (x533 * x565);
	const FLT x568 = (x562 * sensor_pt[1]) + (-1 * x558 * sensor_pt[0]) + (x563 * sensor_pt[2]);
	const FLT x569 = (-1 * x533 * x568) + (-1 * x502 * x562) + (x526 * x566) + (x527 * x564);
	const FLT x570 = (x530 * x568) + (-1 * x531 * x564) + (x502 * x558) + (-1 * x526 * x565);
	const FLT x571 = (-1 * x56 * x570) + (x65 * x567) + (x68 * x569);
	const FLT x572 = (-1 * x65 * x569) + (x68 * x567) + (x67 * x570);
	const FLT x573 = x570 + (x57 * x571) + (-1 * x572 * x166);
	const FLT x574 = (-1 * x67 * x567) + (x56 * x569) + (x68 * x570);
	const FLT x575 = x567 + (x574 * x166) + (-1 * x66 * x571);
	const FLT x576 = (x575 * x197) + (x573 * x189);
	const FLT x577 = x569 + (x66 * x572) + (-1 * x57 * x574);
	const FLT x578 = (-1 * x577 * x228) + (x576 * x255);
	const FLT x579 = ((x573 * x258) + (-1 * x575 * x257)) * x259;
	const FLT x580 = (-1 * x579) + (x578 * x254);
	const FLT x581 = (x577 * x209) + (-1 * x207 * (x576 + (x577 * x164)));
	const FLT x582 = x581 * x217;
	const FLT x583 = (x581 * x220) + (x213 * (x582 + (-1 * x581 * x215)));
	const FLT x584 = (x581 * x223) + (x583 * x213);
	const FLT x585 = x579 + (-1 * x277 * ((x581 * x251) + x578 + (-1 * x580 * x275) + (-1 * x273 * ((x243 * ((x584 * x213) + (x581 * x265) + (x581 * x271) + (x213 * (x584 + (x581 * x266) + (x213 * ((x213 * ((-1 * x581 * x269) + x582 + (x581 * x267))) + x583 + (x581 * x270))))))) + (-1 * x580 * x264))) + (x584 * x248)));
	const FLT x586 = (x82 * x478) + (-1 * x479 * (*_x0).Object.Pose.Rot[1]) + (-1 * x477 * (*_x0).Object.Pose.Rot[3]) + (x551 * (*_x0).Object.Pose.Rot[0]);
	const FLT x587 = x487 * x586;
	const FLT x588 = x481 * x587;
	const FLT x589 = x482 * x586;
	const FLT x590 = 1.0/2.0 * x589;
	const FLT x591 = x481 * x586;
	const FLT x592 = x493 * x591;
	const FLT x593 = (x592 * x504) + (-1 * x80 * x590) + (-1 * x489 * x486 * x586) + x500 + (-1 * x491 * x588);
	const FLT x594 = x491 * x591;
	const FLT x595 = (-1 * x494 * x592) + x507 + (-1 * x506) + (-1 * x82 * x590) + x508 + (-1 * x486 * x587) + (x489 * x594) + x509;
	const FLT x596 = 1.0/2.0 * x588;
	const FLT x597 = (x493 * x594) + x561 + (x591 * x512) + (-1 * x485 * x589) + (x82 * x596);
	const FLT x598 = (x593 * sensor_pt[0]) + (-1 * x595 * sensor_pt[2]) + (x597 * sensor_pt[1]);
	const FLT x599 = x517 + (-1 * x489 * x591 * x504) + (-1 * x491 * x589) + (-1 * x519 * x586) + (x80 * x596);
	const FLT x600 = 2 * ((x595 * sensor_pt[1]) + (x597 * sensor_pt[2]) + (-1 * x599 * sensor_pt[0]));
	const FLT x601 = (x600 * x131) + (-1 * x593 * x531) + (x599 * x502) + (-1 * x598 * x526);
	const FLT x602 = (x599 * sensor_pt[2]) + (-1 * x593 * sensor_pt[1]) + (x597 * sensor_pt[0]);
	const FLT x603 = (x598 * x533) + (x595 * x531) + (-1 * x599 * x527) + (-1 * x602 * x530);
	const FLT x604 = (x602 * x526) + (x593 * x527) + (-1 * x595 * x502) + (-1 * x600 * x127);
	const FLT x605 = (-1 * x65 * x604) + (x67 * x601) + (x68 * x603);
	const FLT x606 = (-1 * x67 * x603) + (x56 * x604) + (x68 * x601);
	const FLT x607 = (x66 * x605) + x604 + (-1 * x57 * x606);
	const FLT x608 = (-1 * x56 * x601) + (x65 * x603) + (x68 * x604);
	const FLT x609 = x601 + (x57 * x608) + (-1 * x605 * x166);
	const FLT x610 = x603 + (x606 * x166) + (-1 * x66 * x608);
	const FLT x611 = (x610 * x197) + (x609 * x189);
	const FLT x612 = (x607 * x209) + (-1 * x207 * (x611 + (x607 * x164)));
	const FLT x613 = x612 * x217;
	const FLT x614 = (x612 * x220) + (x213 * (x613 + (-1 * x612 * x215)));
	const FLT x615 = (x612 * x223) + (x614 * x213);
	const FLT x616 = (-1 * x607 * x228) + (x611 * x255);
	const FLT x617 = ((x609 * x258) + (-1 * x610 * x257)) * x259;
	const FLT x618 = (-1 * x617) + (x616 * x254);
	const FLT x619 = x617 + (-1 * x277 * (x616 + (-1 * x618 * x275) + (-1 * x273 * ((x243 * ((x615 * x213) + (x612 * x271) + (x213 * (x615 + (x612 * x266) + (x213 * (x614 + (x213 * ((-1 * x612 * x269) + x613 + (x612 * x267))) + (x612 * x270))))) + (x612 * x265))) + (-1 * x618 * x264))) + (x612 * x251) + (x615 * x248)));
	const FLT x620 = x398 + x375;
	const FLT x621 = -1 * x378;
	const FLT x622 = 1 + (-1 * x379);
	const FLT x623 = x622 + x621;
	const FLT x624 = x382 + x384;
	const FLT x625 = (x624 * x197) + (x623 * x189);
	const FLT x626 = (x620 * x209) + (-1 * x207 * (x625 + (x620 * x164)));
	const FLT x627 = x626 * x217;
	const FLT x628 = (x626 * x220) + (x213 * (x627 + (-1 * x626 * x215)));
	const FLT x629 = (x626 * x223) + (x628 * x213);
	const FLT x630 = (-1 * x620 * x228) + (x625 * x255);
	const FLT x631 = ((x623 * x258) + (-1 * x624 * x257)) * x259;
	const FLT x632 = (-1 * x631) + (x630 * x254);
	const FLT x633 = x626 * x212;
	const FLT x634 = x631 + (-1 * x277 * ((x633 * x236 * x250 * x246) + (x629 * x248) + x630 + (-1 * x632 * x275) + (-1 * x273 * ((x243 * ((x626 * x271) + (x629 * x213) + (x626 * x265) + (x213 * (x629 + (x626 * x266) + (x213 * (x628 + (x213 * ((-1 * x633 * x268) + x627 + (x626 * x267))) + (x626 * x270))))))) + (-1 * x632 * x264)))));
	const FLT x635 = x375 + x373;
	const FLT x636 = x417 + x402;
	const FLT x637 = (x636 * x197) + (x635 * x189);
	const FLT x638 = -1 * x396;
	const FLT x639 = x622 + x638;
	const FLT x640 = (-1 * x639 * x228) + (x637 * x255);
	const FLT x641 = ((x635 * x258) + (-1 * x636 * x257)) * x259;
	const FLT x642 = (-1 * x641) + (x640 * x254);
	const FLT x643 = (x639 * x209) + (-1 * x207 * (x637 + (x639 * x164)));
	const FLT x644 = x643 * x217;
	const FLT x645 = (x643 * x220) + (x213 * (x644 + (-1 * x643 * x215)));
	const FLT x646 = (x643 * x223) + (x645 * x213);
	const FLT x647 = x641 + (-1 * x277 * (x640 + (x643 * x251) + (-1 * x642 * x275) + (-1 * x273 * ((x243 * ((x646 * x213) + (x643 * x271) + (x643 * x265) + (x213 * ((x643 * x266) + x646 + (x213 * (x645 + (x213 * ((-1 * x643 * x269) + x644 + (x643 * x267))) + (x643 * x270))))))) + (-1 * x642 * x264))) + (x646 * x248)));
	const FLT x648 = x402 + x401;
	const FLT x649 = x385 + x382;
	const FLT x650 = 1 + x638 + x621;
	const FLT x651 = (x650 * x197) + (x649 * x189);
	const FLT x652 = (x648 * x209) + (-1 * x207 * (x651 + (x648 * x164)));
	const FLT x653 = x652 * x217;
	const FLT x654 = (x652 * x220) + (x213 * (x653 + (-1 * x652 * x215)));
	const FLT x655 = (x652 * x223) + (x654 * x213);
	const FLT x656 = (-1 * x648 * x228) + (x651 * x255);
	const FLT x657 = ((x649 * x258) + (-1 * x650 * x257)) * x259;
	const FLT x658 = (-1 * x657) + (x656 * x254);
	const FLT x659 = x657 + (-1 * x277 * (x656 + (-1 * x658 * x275) + (-1 * x273 * ((x243 * ((x655 * x213) + (x652 * x271) + (x652 * x265) + (x213 * (x655 + (x652 * x266) + (x213 * ((x213 * ((-1 * x652 * x269) + x653 + (x652 * x267))) + x654 + (x652 * x270))))))) + (-1 * x658 * x264))) + (x655 * x248) + (x652 * x251)));
	const FLT x660 = dt * dt * dt;
	const FLT x661 = 0.5 * x99 * x660;
	const FLT x662 = x89 * x661;
	const FLT x663 = x88 * x88 * x88;
	const FLT x664 = dt * dt * dt * dt;
	const FLT x665 = 2 * x98 * (1. / (x94 * x94));
	const FLT x666 = x664 * x665;
	const FLT x667 = 1.0 * x101;
	const FLT x668 = x97 * (1. / (x94 * sqrt(x94)));
	const FLT x669 = x668 * x667;
	const FLT x670 = x669 * x664;
	const FLT x671 = x88 * x670;
	const FLT x672 = x88 * x666;
	const FLT x673 = 2 * x100;
	const FLT x674 = x85 * x673;
	const FLT x675 = x667 * x104;
	const FLT x676 = x88 * x85;
	const FLT x677 = (-1 * x675 * x676) + (x92 * x671) + (x86 * x671) + (x670 * x663) + (-1 * x86 * x672) + (-1 * x663 * x666) + (-1 * x92 * x672) + (x88 * x674);
	const FLT x678 = x677 * x112;
	const FLT x679 = 1. / (x102 * sqrt(x102));
	const FLT x680 = 1.0/2.0 * x109;
	const FLT x681 = x679 * x680;
	const FLT x682 = x84 * x485;
	const FLT x683 = x679 * x682;
	const FLT x684 = x73 * x105;
	const FLT x685 = x677 * x684;
	const FLT x686 = x88 * x105;
	const FLT x687 = x84 * x491;
	const FLT x688 = x679 * x687;
	const FLT x689 = x677 * x688;
	const FLT x690 = 0.5 * x104;
	const FLT x691 = x690 * x676;
	const FLT x692 = x679 * x101;
	const FLT x693 = x692 * x677;
	const FLT x694 = 1.0/2.0 * x114;
	const FLT x695 = x668 * x660;
	const FLT x696 = x89 * x695;
	const FLT x697 = x661 * x122;
	const FLT x698 = x88 * x73;
	const FLT x699 = x88 * x695;
	const FLT x700 = x73 * x699;
	const FLT x701 = (-1 * x700 * x129) + (x698 * x697);
	const FLT x702 = x91 * x699;
	const FLT x703 = x88 * x91;
	const FLT x704 = x661 * x126;
	const FLT x705 = (-1 * x704 * x703) + (x702 * x110);
	const FLT x706 = x701 + (-1 * x696 * x120) + x705 + (x662 * x130) + (x678 * x681) + (-1 * x694 * x693) + x116 + (-1 * x691 * x125) + (-1 * x683 * x685) + (-1 * x689 * x686);
	const FLT x707 = -1 * x118;
	const FLT x708 = x700 * x110;
	const FLT x709 = x698 * x704;
	const FLT x710 = x679 * x686;
	const FLT x711 = x694 * x710;
	const FLT x712 = (x697 * x703) + (-1 * x702 * x129);
	const FLT x713 = x712 + x709 + (x696 * x125) + (-1 * x708) + (-1 * x693 * x687) + (-1 * x691 * x120) + (-1 * x681 * x685) + (x677 * x711) + x707 + (-1 * x678 * x683) + (-1 * x662 * x115);
	const FLT x714 = x702 * x120;
	const FLT x715 = x694 * x679;
	const FLT x716 = x661 * x130;
	const FLT x717 = x716 * x703;
	const FLT x718 = x680 * x710;
	const FLT x719 = -1 * x123;
	const FLT x720 = x661 * x115;
	const FLT x721 = (x700 * x125) + (-1 * x698 * x720);
	const FLT x722 = x721 + (-1 * x693 * x682) + (x677 * x718) + x719 + (-1 * x662 * x126) + (x696 * x110) + (-1 * x691 * x129) + x714 + (x685 * x715) + (x678 * x688) + (-1 * x717);
	const FLT x723 = x698 * x716;
	const FLT x724 = x703 * x720;
	const FLT x725 = x702 * x125;
	const FLT x726 = x682 * x710;
	const FLT x727 = x700 * x120;
	const FLT x728 = (-1 * x696 * x129) + (x89 * x697) + (-1 * x691 * x110) + x724 + (-1 * x678 * x715) + x108 + (-1 * x693 * x680) + (-1 * x723) + (x689 * x684) + (-1 * x725) + (-1 * x677 * x726) + x727;
	const FLT x729 = (x728 * sensor_pt[1]) + (-1 * x713 * sensor_pt[0]) + (x722 * sensor_pt[2]);
	const FLT x730 = (x706 * sensor_pt[0]) + (-1 * x728 * sensor_pt[2]) + (x722 * sensor_pt[1]);
	const FLT x731 = (x713 * x502) + (-1 * x730 * x526) + (-1 * x706 * x531) + (x729 * x530);
	const FLT x732 = (x713 * sensor_pt[2]) + (-1 * x706 * sensor_pt[1]) + (x722 * sensor_pt[0]);
	const FLT x733 = (x728 * x531) + (-1 * x713 * x527) + (x730 * x533) + (-1 * x732 * x530);
	const FLT x734 = (x732 * x526) + (-1 * x729 * x533) + (-1 * x728 * x502) + (x706 * x527);
	const FLT x735 = (-1 * x65 * x734) + (x67 * x731) + (x68 * x733);
	const FLT x736 = (-1 * x67 * x733) + (x56 * x734) + (x68 * x731);
	const FLT x737 = (x66 * x735) + x734 + (-1 * x57 * x736);
	const FLT x738 = (-1 * x56 * x731) + (x65 * x733) + (x68 * x734);
	const FLT x739 = x731 + (x57 * x738) + (-1 * x735 * x166);
	const FLT x740 = x733 + (x736 * x166) + (-1 * x66 * x738);
	const FLT x741 = (x740 * x197) + (x739 * x189);
	const FLT x742 = (x737 * x209) + (-1 * x207 * (x741 + (x737 * x164)));
	const FLT x743 = (-1 * x737 * x228) + (x741 * x255);
	const FLT x744 = ((x739 * x258) + (-1 * x740 * x257)) * x259;
	const FLT x745 = (-1 * x744) + (x743 * x254);
	const FLT x746 = x742 * x217;
	const FLT x747 = (x742 * x220) + (x213 * (x746 + (-1 * x742 * x215)));
	const FLT x748 = (x742 * x223) + (x747 * x213);
	const FLT x749 = x744 + (-1 * x277 * (x743 + (-1 * x745 * x275) + (x742 * x251) + (x748 * x248) + (-1 * x273 * ((x243 * ((x748 * x213) + (x742 * x271) + (x742 * x265) + (x213 * ((x742 * x266) + x748 + (x213 * (x747 + (x213 * ((-1 * x742 * x269) + x746 + (x742 * x267))) + (x742 * x270))))))) + (-1 * x745 * x264)))));
	const FLT x750 = x92 * x661;
	const FLT x751 = x92 * x695;
	const FLT x752 = x89 * x666;
	const FLT x753 = x89 * x670;
	const FLT x754 = x85 * x111;
	const FLT x755 = x86 * x91;
	const FLT x756 = (x91 * x91 * x91) * x664;
	const FLT x757 = (x91 * x674) + (x669 * x756) + (-1 * x665 * x756) + (x91 * x753) + (-1 * x91 * x752) + (-1 * x667 * x754) + (-1 * x666 * x755) + (x670 * x755);
	const FLT x758 = x688 * x757;
	const FLT x759 = x684 * x757;
	const FLT x760 = x757 * x112;
	const FLT x761 = x692 * x757;
	const FLT x762 = 1.0/2.0 * x761;
	const FLT x763 = 0.5 * x754;
	const FLT x764 = x73 * x91;
	const FLT x765 = x695 * x764;
	const FLT x766 = (-1 * x765 * x129) + (x697 * x764);
	const FLT x767 = x766 + (x681 * x760) + (-1 * x683 * x759) + (-1 * x762 * x114) + x719 + (-1 * x750 * x126) + (x751 * x110) + (-1 * x763 * x125) + (-1 * x714) + (-1 * x686 * x758) + x717;
	const FLT x768 = x765 * x110;
	const FLT x769 = x764 * x704;
	const FLT x770 = (x711 * x757) + x725 + (-1 * x768) + x108 + (-1 * x683 * x760) + (-1 * x751 * x129) + (-1 * x724) + (-1 * x687 * x761) + (-1 * x681 * x759) + (x92 * x697) + (-1 * x763 * x120) + x769;
	const FLT x771 = -1 * x116;
	const FLT x772 = x84 * x762;
	const FLT x773 = x765 * x125;
	const FLT x774 = x764 * x720;
	const FLT x775 = x705 + (-1 * x763 * x129) + (-1 * x750 * x130) + (x758 * x112) + (x715 * x759) + x771 + (-1 * x774) + (-1 * x77 * x772) + (x751 * x120) + (x718 * x757) + x773;
	const FLT x776 = (-1 * x764 * x716) + (x765 * x120);
	const FLT x777 = x712 + (-1 * x763 * x110) + (-1 * x82 * x772) + x118 + (-1 * x726 * x757) + (x684 * x758) + (x750 * x115) + x776 + (-1 * x760 * x715) + (-1 * x751 * x125);
	const FLT x778 = (-1 * x770 * sensor_pt[0]) + (x777 * sensor_pt[1]) + (x775 * sensor_pt[2]);
	const FLT x779 = (x767 * sensor_pt[0]) + (-1 * x777 * sensor_pt[2]) + (x775 * sensor_pt[1]);
	const FLT x780 = (x770 * x502) + (-1 * x767 * x531) + (-1 * x779 * x526) + (x778 * x530);
	const FLT x781 = (x770 * sensor_pt[2]) + (-1 * x767 * sensor_pt[1]) + (x775 * sensor_pt[0]);
	const FLT x782 = (x781 * x526) + (-1 * x778 * x533) + (-1 * x777 * x502) + (x767 * x527);
	const FLT x783 = (x779 * x533) + (-1 * x781 * x530) + (-1 * x770 * x527) + (x777 * x531);
	const FLT x784 = (-1 * x67 * x783) + (x68 * x780) + (x56 * x782);
	const FLT x785 = (-1 * x65 * x782) + (x67 * x780) + (x68 * x783);
	const FLT x786 = (-1 * x57 * x784) + x782 + (x66 * x785);
	const FLT x787 = (-1 * x56 * x780) + (x65 * x783) + (x68 * x782);
	const FLT x788 = x780 + (x57 * x787) + (-1 * x785 * x166);
	const FLT x789 = x783 + (x784 * x166) + (-1 * x66 * x787);
	const FLT x790 = (x789 * x197) + (x788 * x189);
	const FLT x791 = (x786 * x209) + (-1 * x207 * (x790 + (x786 * x164)));
	const FLT x792 = x791 * x217;
	const FLT x793 = (x791 * x220) + (x213 * (x792 + (-1 * x791 * x215)));
	const FLT x794 = (x791 * x223) + (x793 * x213);
	const FLT x795 = (-1 * x786 * x228) + (x790 * x255);
	const FLT x796 = ((x788 * x258) + (-1 * x789 * x257)) * x259;
	const FLT x797 = (-1 * x796) + (x795 * x254);
	const FLT x798 = x796 + (-1 * x277 * ((-1 * x797 * x275) + (-1 * x273 * ((x243 * ((x794 * x213) + (x791 * x265) + (x213 * (x794 + (x791 * x266) + (x213 * (x793 + (x213 * ((-1 * x791 * x269) + x792 + (x791 * x267))) + (x791 * x270))))) + (x791 * x271))) + (-1 * x797 * x264))) + (x794 * x248) + x795 + (x791 * x251)));
	const FLT x799 = x85 * x73;
	const FLT x800 = x73 * x92;
	const FLT x801 = x73 * x73 * x73;
	const FLT x802 = (x670 * x801) + (-1 * x675 * x799) + (-1 * x666 * x801) + (x73 * x753) + (x673 * x799) + (x670 * x800) + (-1 * x73 * x752) + (-1 * x666 * x800);
	const FLT x803 = x684 * x802;
	const FLT x804 = x86 * x695;
	const FLT x805 = x802 * x112;
	const FLT x806 = x690 * x799;
	const FLT x807 = x692 * x802;
	const FLT x808 = 1.0/2.0 * x807;
	const FLT x809 = x688 * x802;
	const FLT x810 = (-1 * x686 * x809) + (-1 * x808 * x114) + x108 + (-1 * x683 * x803) + (-1 * x727) + x723 + (-1 * x769) + x768 + (x681 * x805) + (x86 * x697) + (-1 * x804 * x129) + (-1 * x806 * x125);
	const FLT x811 = x86 * x661;
	const FLT x812 = x701 + x774 + (-1 * x715 * x805) + (-1 * x808 * x109) + x771 + (x684 * x809) + (-1 * x811 * x130) + (-1 * x773) + (x804 * x120) + (-1 * x726 * x802) + (-1 * x806 * x110);
	const FLT x813 = x776 + (x809 * x112) + (x804 * x125) + (x718 * x802) + (-1 * x811 * x115) + x707 + (-1 * x682 * x807) + (-1 * x806 * x129) + (-1 * x709) + (x715 * x803) + x708;
	const FLT x814 = (x810 * sensor_pt[0]) + (-1 * x812 * sensor_pt[2]) + (x813 * sensor_pt[1]);
	const FLT x815 = x721 + (-1 * x687 * x807) + x766 + x123 + (-1 * x683 * x805) + (-1 * x806 * x120) + (x811 * x126) + (-1 * x804 * x110) + (x711 * x802) + (-1 * x681 * x803);
	const FLT x816 = (x813 * sensor_pt[2]) + (x812 * sensor_pt[1]) + (-1 * x815 * sensor_pt[0]);
	const FLT x817 = (x530 * x816) + (x502 * x815) + (-1 * x531 * x810) + (-1 * x526 * x814);
	const FLT x818 = (-1 * x810 * sensor_pt[1]) + (x815 * sensor_pt[2]) + (x813 * sensor_pt[0]);
	const FLT x819 = (-1 * x530 * x818) + (x533 * x814) + (-1 * x527 * x815) + (x531 * x812);
	const FLT x820 = (x526 * x818) + (-1 * x533 * x816) + (-1 * x502 * x812) + (x527 * x810);
	const FLT x821 = (x67 * x817) + (-1 * x65 * x820) + (x68 * x819);
	const FLT x822 = (-1 * x67 * x819) + (x56 * x820) + (x68 * x817);
	const FLT x823 = x820 + (x66 * x821) + (-1 * x57 * x822);
	const FLT x824 = (-1 * x56 * x817) + (x65 * x819) + (x68 * x820);
	const FLT x825 = x817 + (x57 * x824) + (-1 * x821 * x166);
	const FLT x826 = x819 + (x822 * x166) + (-1 * x66 * x824);
	const FLT x827 = (x826 * x197) + (x825 * x189);
	const FLT x828 = (x823 * x209) + (-1 * x207 * (x827 + (x823 * x164)));
	const FLT x829 = x828 * x217;
	const FLT x830 = (x828 * x220) + (x213 * (x829 + (-1 * x828 * x215)));
	const FLT x831 = (x828 * x223) + (x830 * x213);
	const FLT x832 = (-1 * x823 * x228) + (x827 * x255);
	const FLT x833 = ((x825 * x258) + (-1 * x826 * x257)) * x259;
	const FLT x834 = x262 * ((-1 * x833) + (x832 * x254));
	const FLT x835 = x833 + (-1 * x277 * ((-1 * x834 * x274) + (x828 * x251) + x832 + (-1 * x273 * ((x243 * ((x213 * (x831 + (x828 * x266) + (x213 * (x830 + (x213 * ((-1 * x828 * x269) + (x828 * x267) + x829)) + (x828 * x270))))) + (x831 * x213) + (x828 * x265) + (x828 * x271))) + (-1 * x834 * x263))) + (x831 * x248)));
	const FLT x836 = -1 * dt * x378;
	const FLT x837 = (-1 * dt * x379) + dt;
	const FLT x838 = x837 + x836;
	const FLT x839 = dt * x384;
	const FLT x840 = dt * x382;
	const FLT x841 = x840 + x839;
	const FLT x842 = (x841 * x197) + (x838 * x189);
	const FLT x843 = dt * x375;
	const FLT x844 = dt * x373;
	const FLT x845 = (-1 * x844) + x843;
	const FLT x846 = (-1 * x845 * x228) + (x842 * x255);
	const FLT x847 = ((x838 * x258) + (-1 * x841 * x257)) * x259;
	const FLT x848 = (-1 * x847) + (x846 * x254);
	const FLT x849 = (x845 * x209) + (-1 * x207 * (x842 + (x845 * x164)));
	const FLT x850 = x849 * x217;
	const FLT x851 = (x849 * x220) + (x213 * (x850 + (-1 * x849 * x215)));
	const FLT x852 = (x849 * x223) + (x851 * x213);
	const FLT x853 = x847 + (-1 * x277 * (x846 + (-1 * x848 * x275) + (x849 * x251) + (-1 * x273 * ((x243 * ((x852 * x213) + (x849 * x271) + (x213 * (x852 + (x849 * x266) + (x213 * (x851 + (x213 * (x850 + (-1 * x849 * x269) + (x849 * x267))) + (x849 * x270))))) + (x849 * x265))) + (-1 * x848 * x264))) + (x852 * x248)));
	const FLT x854 = -1 * dt * x396;
	const FLT x855 = x837 + x854;
	const FLT x856 = x843 + x844;
	const FLT x857 = dt * x402;
	const FLT x858 = dt * x401;
	const FLT x859 = (-1 * x858) + x857;
	const FLT x860 = (x859 * x197) + (x856 * x189);
	const FLT x861 = (x855 * x209) + (-1 * x207 * (x860 + (x855 * x164)));
	const FLT x862 = (-1 * x855 * x228) + (x860 * x255);
	const FLT x863 = ((x856 * x258) + (-1 * x859 * x257)) * x259;
	const FLT x864 = (-1 * x863) + (x862 * x254);
	const FLT x865 = x861 * x217;
	const FLT x866 = (x861 * x220) + (x213 * (x865 + (-1 * x861 * x215)));
	const FLT x867 = (x861 * x223) + (x866 * x213);
	const FLT x868 = x863 + (-1 * x277 * ((-1 * x864 * x275) + (x861 * x251) + x862 + (x867 * x248) + (-1 * x273 * ((x243 * ((x867 * x213) + (x861 * x265) + (x213 * (x867 + (x861 * x266) + (x213 * (x866 + (x213 * ((-1 * x861 * x269) + x865 + (x861 * x267))) + (x861 * x270))))) + (x861 * x271))) + (-1 * x864 * x264)))));
	const FLT x869 = x857 + x858;
	const FLT x870 = (-1 * x839) + x840;
	const FLT x871 = x836 + x854 + dt;
	const FLT x872 = (x871 * x197) + (x870 * x189);
	const FLT x873 = (x869 * x209) + (-1 * x207 * (x872 + (x869 * x164)));
	const FLT x874 = (-1 * x869 * x228) + (x872 * x255);
	const FLT x875 = ((x870 * x258) + (-1 * x871 * x257)) * x259;
	const FLT x876 = (-1 * x875) + (x874 * x254);
	const FLT x877 = x873 * x217;
	const FLT x878 = (x873 * x220) + (x213 * (x877 + (-1 * x873 * x215)));
	const FLT x879 = (x873 * x223) + (x878 * x213);
	const FLT x880 = x875 + (-1 * x277 * ((-1 * x876 * x275) + (x873 * x251) + x874 + (x879 * x248) + (-1 * x273 * ((x243 * ((x873 * x271) + (x213 * (x879 + (x873 * x266) + (x213 * (x878 + (x213 * ((-1 * x873 * x269) + x877 + (x873 * x267))) + (x873 * x270))))) + (x879 * x213) + (x873 * x265))) + (-1 * x876 * x264)))));
	const FLT x881 = x272 * x244;
	const FLT x882 = ((-1 * x881) + x274) * x277;
	const FLT x883 = ((x881 * x231) + (-1 * x231 * x274)) * x277;
	const FLT x884 = x277 * ((-1 * x881 * x262) + x275);
	const FLT x885 = x226 * x163 * (1 + x253);
	const FLT x886 = x885 * x254;
	const FLT x887 = x211 * x208;
	const FLT x888 = x887 * x242 * x163;
	const FLT x889 = -1 * x888 * x217;
	const FLT x890 = (-1 * x888 * x220) + (x213 * (x889 + (x888 * x215)));
	const FLT x891 = (-1 * x888 * x223) + (x890 * x213);
	const FLT x892 = x277 * ((x891 * x248) + x885 + (-1 * x886 * x275) + (-1 * x887 * x243 * x249 * x164) + (-1 * x273 * ((-1 * x233 * x200 * x241) + (x243 * ((x891 * x213) + (-1 * x888 * x271) + (-1 * x888 * x265) + (x213 * (x891 + (-1 * x888 * x266) + (x213 * (x890 + (x213 * ((x888 * x269) + x889 + (-1 * x888 * x267))) + (-1 * x888 * x270))))))) + x242 + (-1 * x886 * x264))));
	cnSetZero(Hx);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[0])/sizeof(FLT), x278 + (x278 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[1])/sizeof(FLT), x331 + (x280 * x331));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.AxisAngleRot[2])/sizeof(FLT), x368 + (x280 * x368));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[0])/sizeof(FLT), x395 + (x280 * x395));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[1])/sizeof(FLT), x413 + (x413 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Lighthouse.Pos[2])/sizeof(FLT), x426 + (x426 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[0])/sizeof(FLT), x448 + (x448 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[1])/sizeof(FLT), x463 + (x463 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Acc[2])/sizeof(FLT), x475 + (x475 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[0])/sizeof(FLT), x550 + (x550 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[1])/sizeof(FLT), x585 + (x585 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.AxisAngleRot[2])/sizeof(FLT), x619 + (x619 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[0])/sizeof(FLT), x634 + (x634 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[1])/sizeof(FLT), x647 + (x647 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Pose.Pos[2])/sizeof(FLT), x659 + (x659 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[0])/sizeof(FLT), x749 + (x749 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[1])/sizeof(FLT), x798 + (x798 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.AxisAngleRot[2])/sizeof(FLT), x835 + (x835 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[0])/sizeof(FLT), x853 + (x853 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[1])/sizeof(FLT), x868 + (x868 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, Object.Velocity.Pos[2])/sizeof(FLT), x880 + (x880 * x280));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.curve)/sizeof(FLT), (-1 * x882 * x280) + (-1 * x882));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.gibmag)/sizeof(FLT), sin(x279));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.gibpha)/sizeof(FLT), x280);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeemag)/sizeof(FLT), (-1 * x883 * x280) + (-1 * x883));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.ogeephase)/sizeof(FLT), (-1 * x884 * x280) + (-1 * x884));
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.phase)/sizeof(FLT), -1);
	cnMatrixOptionalSet(Hx, 0, offsetof(SurviveJointKalmanErrorModel, BSD1.tilt)/sizeof(FLT), (-1 * x892 * x280) + (-1 * x892));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [(*error_model).Lighthouse.AxisAngleRot[0], (*error_model).Lighthouse.AxisAngleRot[1], (*error_model).Lighthouse.AxisAngleRot[2], (*error_model).Lighthouse.Pos[0], (*error_model).Lighthouse.Pos[1], (*error_model).Lighthouse.Pos[2], (*error_model).Object.Acc[0], (*error_model).Object.Acc[1], (*error_model).Object.Acc[2], (*error_model).Object.IMUBias.AccBias[0], (*error_model).Object.IMUBias.AccBias[1], (*error_model).Object.IMUBias.AccBias[2], (*error_model).Object.IMUBias.AccScale[0], (*error_model).Object.IMUBias.AccScale[1], (*error_model).Object.IMUBias.AccScale[2], (*error_model).Object.IMUBias.GyroBias[0], (*error_model).Object.IMUBias.GyroBias[1], (*error_model).Object.IMUBias.GyroBias[2], (*error_model).Object.IMUBias.IMUCorrection[0], (*error_model).Object.IMUBias.IMUCorrection[1], (*error_model).Object.IMUBias.IMUCorrection[2], (*error_model).Object.Pose.AxisAngleRot[0], (*error_model).Object.Pose.AxisAngleRot[1], (*error_model).Object.Pose.AxisAngleRot[2], (*error_model).Object.Pose.Pos[0], (*error_model).Object.Pose.Pos[1], (*error_model).Object.Pose.Pos[2], (*error_model).Object.Velocity.AxisAngleRot[0], (*error_model).Object.Velocity.AxisAngleRot[1], (*error_model).Object.Velocity.AxisAngleRot[2], (*error_model).Object.Velocity.Pos[0], (*error_model).Object.Velocity.Pos[1], (*error_model).Object.Velocity.Pos[2], curve, curve, gibmag, gibmag, gibpha, gibpha, ogeemag, ogeemag, ogeephase, ogeephase, phase, phase, tilt, tilt]

static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_error_model_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_error_model(Hx, dt, _x0, error_model, sensor_pt);
    }
}
// Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]]
static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_sensor_pt(CnMat* Hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
	const FLT x0 = 0.5 * (*_x0).Object.Pose.Rot[3];
	const FLT x1 = 0.5 * (*_x0).Object.Pose.Rot[2];
	const FLT x2 = 0.5 * (*_x0).Object.Pose.Rot[1];
	const FLT x3 = (*_x0).Object.Pose.Rot[0] + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[2]) + (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x1 * (*error_model).Object.Pose.AxisAngleRot[1]);
	const FLT x4 = (*_x0).Object.Velocity.AxisAngleRot[1] + (*error_model).Object.Velocity.AxisAngleRot[1];
	const FLT x5 = dt * dt;
	const FLT x6 = (*_x0).Object.Velocity.AxisAngleRot[2] + (*error_model).Object.Velocity.AxisAngleRot[2];
	const FLT x7 = (x6 * x6) * x5;
	const FLT x8 = (*_x0).Object.Velocity.AxisAngleRot[0] + (*error_model).Object.Velocity.AxisAngleRot[0];
	const FLT x9 = x5 * (x8 * x8);
	const FLT x10 = (x4 * x4) * x5;
	const FLT x11 = 1e-10 + x7 + x10 + x9;
	const FLT x12 = sqrt(x11);
	const FLT x13 = 0.5 * x12;
	const FLT x14 = sin(x13);
	const FLT x15 = 0.5 * (*_x0).Object.Pose.Rot[0];
	const FLT x16 = (x2 * (*error_model).Object.Pose.AxisAngleRot[1]) + (-1 * x1 * (*error_model).Object.Pose.AxisAngleRot[0]) + (*_x0).Object.Pose.Rot[3] + (x15 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x17 = (-1 * x2 * (*error_model).Object.Pose.AxisAngleRot[2]) + (x0 * (*error_model).Object.Pose.AxisAngleRot[0]) + (x15 * (*error_model).Object.Pose.AxisAngleRot[1]) + (*_x0).Object.Pose.Rot[2];
	const FLT x18 = (*_x0).Object.Pose.Rot[1] + (x15 * (*error_model).Object.Pose.AxisAngleRot[0]) + (-1 * x0 * (*error_model).Object.Pose.AxisAngleRot[1]) + (x1 * (*error_model).Object.Pose.AxisAngleRot[2]);
	const FLT x19 = (x14 * x14) * (1. / x11);
	const FLT x20 = cos(x13);
	const FLT x21 = (1. / sqrt((x3 * x3) + (x16 * x16) + (x18 * x18) + (x17 * x17))) * (1. / sqrt((x20 * x20) + (x9 * x19) + (x7 * x19) + (x10 * x19)));
	const FLT x22 = dt * x21 * x14 * (1. / x12);
	const FLT x23 = x4 * x22;
	const FLT x24 = x3 * x23;
	const FLT x25 = x20 * x21;
	const FLT x26 = x25 * x17;
	const FLT x27 = x6 * x22;
	const FLT x28 = x27 * x18;
	const FLT x29 = x8 * x22;
	const FLT x30 = x29 * x16;
	const FLT x31 = x28 + (-1 * x30) + x24 + x26;
	const FLT x32 = (x3 * x25) + (-1 * x27 * x16) + (-1 * x29 * x18) + (-1 * x23 * x17);
	const FLT x33 = x3 * x27;
	const FLT x34 = x23 * x18;
	const FLT x35 = x25 * x16;
	const FLT x36 = x29 * x17;
	const FLT x37 = x36 + x35 + x33 + (-1 * x34);
	const FLT x38 = (-1 * x37 * sensor_pt[1]) + (x32 * sensor_pt[0]) + (x31 * sensor_pt[2]);
	const FLT x39 = x27 * x17;
	const FLT x40 = x23 * x16;
	const FLT x41 = x25 * x18;
	const FLT x42 = x3 * x29;
	const FLT x43 = x42 + x41 + (-1 * x39) + x40;
	const FLT x44 = (-1 * x43 * sensor_pt[2]) + (x32 * sensor_pt[1]) + (x37 * sensor_pt[0]);
	const FLT x45 = 1.0/2.0 * dt * fabs(dt);
	const FLT x46 = (*_x0).Object.Pose.Pos[2] + (2 * ((x43 * x44) + (-1 * x31 * x38))) + sensor_pt[2] + (*error_model).Object.Pose.Pos[2] + (dt * ((*_x0).Object.Velocity.Pos[2] + (*error_model).Object.Velocity.Pos[2])) + (x45 * ((*_x0).Object.Acc[2] + (*error_model).Object.Acc[2]));
	const FLT x47 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[2];
	const FLT x48 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[1];
	const FLT x49 = 0.5 * (*error_model).Lighthouse.AxisAngleRot[0];
	const FLT x50 = (-1 * x49 * (*_x0).Lighthouse.Rot[1]) + (*_x0).Lighthouse.Rot[0] + (-1 * x47 * (*_x0).Lighthouse.Rot[3]) + (-1 * x48 * (*_x0).Lighthouse.Rot[2]);
	const FLT x51 = 0.5 * (*_x0).Lighthouse.Rot[0];
	const FLT x52 = 0.5 * (*_x0).Lighthouse.Rot[1];
	const FLT x53 = (x52 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x51 * (*error_model).Lighthouse.AxisAngleRot[2]) + (*_x0).Lighthouse.Rot[3] + (-1 * x49 * (*_x0).Lighthouse.Rot[2]);
	const FLT x54 = x53 * x53;
	const FLT x55 = x50 * x50;
	const FLT x56 = (x51 * (*error_model).Lighthouse.AxisAngleRot[1]) + (x49 * (*_x0).Lighthouse.Rot[3]) + (-1 * x52 * (*error_model).Lighthouse.AxisAngleRot[2]) + (*_x0).Lighthouse.Rot[2];
	const FLT x57 = x56 * x56;
	const FLT x58 = (*_x0).Lighthouse.Rot[1] + (-1 * x48 * (*_x0).Lighthouse.Rot[3]) + (x51 * (*error_model).Lighthouse.AxisAngleRot[0]) + (x47 * (*_x0).Lighthouse.Rot[2]);
	const FLT x59 = x58 * x58;
	const FLT x60 = x59 + x57 + x54 + x55;
	const FLT x61 = 1. / x60;
	const FLT x62 = (1. / sqrt((x61 * x59) + (x61 * x55) + (x61 * x54) + (x61 * x57))) * (1. / sqrt(x60));
	const FLT x63 = x62 * x50;
	const FLT x64 = (-1 * x31 * sensor_pt[0]) + (x32 * sensor_pt[2]) + (x43 * sensor_pt[1]);
	const FLT x65 = (x45 * ((*_x0).Object.Acc[1] + (*error_model).Object.Acc[1])) + (*error_model).Object.Pose.Pos[1] + sensor_pt[1] + (dt * ((*_x0).Object.Velocity.Pos[1] + (*error_model).Object.Velocity.Pos[1])) + (*_x0).Object.Pose.Pos[1] + (2 * ((x38 * x37) + (-1 * x64 * x43)));
	const FLT x66 = x62 * x58;
	const FLT x67 = (*_x0).Object.Pose.Pos[0] + (x45 * ((*_x0).Object.Acc[0] + (*error_model).Object.Acc[0])) + (*error_model).Object.Pose.Pos[0] + (2 * ((x64 * x31) + (-1 * x44 * x37))) + (dt * ((*_x0).Object.Velocity.Pos[0] + (*error_model).Object.Velocity.Pos[0])) + sensor_pt[0];
	const FLT x68 = x62 * x56;
	const FLT x69 = (x67 * x68) + (x63 * x46) + (-1 * x65 * x66);
	const FLT x70 = x62 * x53;
	const FLT x71 = (x63 * x67) + (x70 * x65) + (-1 * x68 * x46);
	const FLT x72 = (*_x0).Lighthouse.Pos[2] + (*error_model).Lighthouse.Pos[2];
	const FLT x73 = (*_x0).Lighthouse.Pos[1] + (*error_model).Lighthouse.Pos[1];
	const FLT x74 = (*_x0).Lighthouse.Pos[0] + (*error_model).Lighthouse.Pos[0];
	const FLT x75 = (x74 * x68) + (x72 * x63) + (-1 * x73 * x66);
	const FLT x76 = (x70 * x73) + (-1 * x72 * x68) + (x74 * x63);
	const FLT x77 = x65 + (2 * ((-1 * x71 * x70) + (x66 * x69))) + (-1 * (x73 + (2 * ((-1 * x70 * x76) + (x75 * x66)))));
	const FLT x78 = x77 * x77;
	const FLT x79 = (x72 * x66) + (x73 * x63) + (-1 * x70 * x74);
	const FLT x80 = (x66 * x46) + (x63 * x65) + (-1 * x70 * x67);
	const FLT x81 = x46 + (-1 * (x72 + (2 * ((-1 * x79 * x66) + (x76 * x68))))) + (2 * ((-1 * x80 * x66) + (x71 * x68)));
	const FLT x82 = x67 + (-1 * (x74 + (2 * ((-1 * x75 * x68) + (x70 * x79))))) + (2 * ((-1 * x68 * x69) + (x80 * x70)));
	const FLT x83 = x82 * x82;
	const FLT x84 = x83 + (x81 * x81);
	const FLT x85 = x84 + x78;
	const FLT x86 = 0.523598775598299 + (-1 * (*error_model).BSD1.tilt) + (-1 * (*_x0).BSD1.tilt);
	const FLT x87 = cos(x86);
	const FLT x88 = 1. / x87;
	const FLT x89 = x88 * (1. / sqrt(x85));
	const FLT x90 = asin(x89 * x77);
	const FLT x91 = 8.0108022e-06 * x90;
	const FLT x92 = -8.0108022e-06 + (-1 * x91);
	const FLT x93 = 0.0028679863 + (x92 * x90);
	const FLT x94 = 5.3685255e-06 + (x93 * x90);
	const FLT x95 = 0.0076069798 + (x90 * x94);
	const FLT x96 = x90 * x95;
	const FLT x97 = -8.0108022e-06 + (-1.60216044e-05 * x90);
	const FLT x98 = x93 + (x90 * x97);
	const FLT x99 = x94 + (x90 * x98);
	const FLT x100 = x95 + (x90 * x99);
	const FLT x101 = (x90 * x100) + x96;
	const FLT x102 = atan2(-1 * x81, x82);
	const FLT x103 = tan(x86);
	const FLT x104 = (1. / sqrt(x84)) * x103;
	const FLT x105 = -1 * x77 * x104;
	const FLT x106 = asin(x105) + (-1 * (*error_model).BSD1.ogeephase) + (-1 * (*_x0).BSD1.ogeephase) + (-1 * x102);
	const FLT x107 = (*error_model).BSD1.ogeemag + (*_x0).BSD1.ogeemag;
	const FLT x108 = (*_x0).BSD1.curve + (-1 * x107 * sin(x106)) + (*error_model).BSD1.curve;
	const FLT x109 = sin(x86);
	const FLT x110 = x109 * x108;
	const FLT x111 = x87 + (x101 * x110);
	const FLT x112 = 1. / x111;
	const FLT x113 = x90 * x90;
	const FLT x114 = x108 * x113;
	const FLT x115 = x112 * x114;
	const FLT x116 = x105 + (x95 * x115);
	const FLT x117 = 1. / sqrt(1 + (-1 * (x116 * x116)));
	const FLT x118 = 1. / sqrt(1 + (-1 * (1. / x85) * (1. / (x87 * x87)) * x78));
	const FLT x119 = (-1 * x28) + (-1 * x26) + x30 + (-1 * x24);
	const FLT x120 = 2 * x43;
	const FLT x121 = 2 * x37;
	const FLT x122 = x32 * x121;
	const FLT x123 = x122 + (-1 * x119 * x120);
	const FLT x124 = 2 * x31;
	const FLT x125 = 1 + (x119 * x124) + (-2 * (x37 * x37));
	const FLT x126 = x32 * x124;
	const FLT x127 = (x37 * x120) + (-1 * x126);
	const FLT x128 = (-1 * x68 * x127) + (x70 * x123) + (x63 * x125);
	const FLT x129 = 2 * x70;
	const FLT x130 = 2 * ((-1 * x66 * x123) + (x68 * x125) + (x63 * x127));
	const FLT x131 = x123 + (-1 * x128 * x129) + (x66 * x130);
	const FLT x132 = 2 * x77;
	const FLT x133 = (-1 * x70 * x125) + (x66 * x127) + (x63 * x123);
	const FLT x134 = x125 + (-1 * x68 * x130) + (x129 * x133);
	const FLT x135 = 2 * x82;
	const FLT x136 = 2 * x68;
	const FLT x137 = 2 * x66;
	const FLT x138 = x127 + (x128 * x136) + (-1 * x133 * x137);
	const FLT x139 = 2 * x81;
	const FLT x140 = (x138 * x139) + (x134 * x135);
	const FLT x141 = 1.0/2.0 * x77;
	const FLT x142 = x88 * (1. / (x85 * sqrt(x85))) * x141;
	const FLT x143 = x118 * ((x89 * x131) + (-1 * x142 * (x140 + (x131 * x132))));
	const FLT x144 = x92 * x143;
	const FLT x145 = (x93 * x143) + (x90 * (x144 + (-1 * x91 * x143)));
	const FLT x146 = (x94 * x143) + (x90 * x145);
	const FLT x147 = 2 * x96 * x108 * x112;
	const FLT x148 = 1. / x84;
	const FLT x149 = 1. / sqrt(1 + (-1 * x78 * (x103 * x103) * x148));
	const FLT x150 = (1. / (x84 * sqrt(x84))) * x103 * x141;
	const FLT x151 = (-1 * x104 * x131) + (x140 * x150);
	const FLT x152 = 1. / x82;
	const FLT x153 = x81 * (1. / x83);
	const FLT x154 = x83 * x148;
	const FLT x155 = ((x134 * x153) + (-1 * x138 * x152)) * x154;
	const FLT x156 = (-1 * x155) + (x149 * x151);
	const FLT x157 = x107 * cos(x106);
	const FLT x158 = x101 * x109;
	const FLT x159 = x157 * x158;
	const FLT x160 = 2.40324066e-05 * x90;
	const FLT x161 = x95 * (1. / (x111 * x111)) * x114;
	const FLT x162 = x95 * x112 * x113;
	const FLT x163 = x162 * x157;
	const FLT x164 = x155 + (-1 * x117 * (x151 + (-1 * x163 * x156) + (x115 * x146) + (-1 * x161 * ((x110 * ((x90 * x146) + (x90 * (x146 + (x99 * x143) + (x90 * (x145 + (x90 * (x144 + (-1 * x160 * x143) + (x97 * x143))) + (x98 * x143))))) + (x100 * x143) + (x95 * x143))) + (-1 * x156 * x159))) + (x143 * x147)));
	const FLT x165 = ((*error_model).BSD1.gibmag + (*_x0).BSD1.gibmag) * cos((-1 * asin(x116)) + x102 + (*error_model).BSD1.gibpha + (*_x0).BSD1.gibpha);
	const FLT x166 = (-1 * x33) + (-1 * x36) + x34 + (-1 * x35);
	const FLT x167 = 1 + (x121 * x166) + (-2 * (x43 * x43));
	const FLT x168 = (x31 * x120) + (-1 * x122);
	const FLT x169 = x32 * x120;
	const FLT x170 = x169 + (-1 * x124 * x166);
	const FLT x171 = (-1 * x68 * x170) + (x70 * x167) + (x63 * x168);
	const FLT x172 = (-1 * x66 * x167) + (x68 * x168) + (x63 * x170);
	const FLT x173 = (-1 * x129 * x171) + x167 + (x172 * x137);
	const FLT x174 = (-1 * x70 * x168) + (x66 * x170) + (x63 * x167);
	const FLT x175 = (-1 * x172 * x136) + x168 + (x129 * x174);
	const FLT x176 = x170 + (x171 * x136) + (-1 * x174 * x137);
	const FLT x177 = (x176 * x139) + (x175 * x135);
	const FLT x178 = x118 * ((x89 * x173) + (-1 * x142 * (x177 + (x173 * x132))));
	const FLT x179 = x92 * x178;
	const FLT x180 = (x93 * x178) + (x90 * (x179 + (-1 * x91 * x178)));
	const FLT x181 = (x94 * x178) + (x90 * x180);
	const FLT x182 = (-1 * x104 * x173) + (x177 * x150);
	const FLT x183 = ((x175 * x153) + (-1 * x176 * x152)) * x154;
	const FLT x184 = x157 * ((-1 * x183) + (x182 * x149));
	const FLT x185 = x183 + (-1 * x117 * (x182 + (-1 * x161 * ((x110 * ((x100 * x178) + (x90 * (x181 + (x99 * x178) + (x90 * (x180 + (x90 * ((-1 * x160 * x178) + x179 + (x97 * x178))) + (x98 * x178))))) + (x90 * x181) + (x95 * x178))) + (-1 * x184 * x158))) + (x115 * x181) + (-1 * x162 * x184) + (x178 * x147)));
	const FLT x186 = (x37 * x124) + (-1 * x169);
	const FLT x187 = (-1 * x41) + (-1 * x42) + (-1 * x40) + x39;
	const FLT x188 = x126 + (-1 * x121 * x187);
	const FLT x189 = 1 + (x120 * x187) + (-2 * (x31 * x31));
	const FLT x190 = (-1 * x68 * x189) + (x70 * x186) + (x63 * x188);
	const FLT x191 = (x68 * x188) + (-1 * x66 * x186) + (x63 * x189);
	const FLT x192 = x186 + (-1 * x129 * x190) + (x191 * x137);
	const FLT x193 = (-1 * x70 * x188) + (x66 * x189) + (x63 * x186);
	const FLT x194 = x188 + (-1 * x191 * x136) + (x129 * x193);
	const FLT x195 = x189 + (x190 * x136) + (-1 * x193 * x137);
	const FLT x196 = (x195 * x139) + (x194 * x135);
	const FLT x197 = x118 * ((x89 * x192) + (-1 * x142 * (x196 + (x192 * x132))));
	const FLT x198 = (-1 * x104 * x192) + (x196 * x150);
	const FLT x199 = ((x194 * x153) + (-1 * x195 * x152)) * x154;
	const FLT x200 = (-1 * x199) + (x198 * x149);
	const FLT x201 = x92 * x197;
	const FLT x202 = (x93 * x197) + (x90 * (x201 + (-1 * x91 * x197)));
	const FLT x203 = (x94 * x197) + (x90 * x202);
	const FLT x204 = x199 + (-1 * x117 * ((x203 * x115) + x198 + (x197 * x147) + (-1 * x200 * x163) + (-1 * x161 * ((x110 * ((x90 * x203) + (x100 * x197) + (x95 * x197) + (x90 * (x203 + (x99 * x197) + (x90 * (x202 + (x90 * ((-1 * x160 * x197) + x201 + (x97 * x197))) + (x98 * x197))))))) + (-1 * x200 * x159)))));
	cnMatrixOptionalSet(Hx, 0, 0, x164 + (x165 * x164));
	cnMatrixOptionalSet(Hx, 0, 1, x185 + (x165 * x185));
	cnMatrixOptionalSet(Hx, 0, 2, x204 + (x204 * x165));
}

// Full version Jacobian of SurviveJointKalmanErrorModel_LightMeas_y_gen2 wrt [sensor_pt[0], sensor_pt[1], sensor_pt[2]]

static inline void SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_sensor_pt_with_hx(CnMat* Hx, CnMat* hx, const FLT dt, const SurviveJointKalmanModel* _x0, const SurviveJointKalmanErrorModel* error_model, const FLT* sensor_pt) {
    if(hx != 0) { 
        hx->data[0] = SurviveJointKalmanErrorModel_LightMeas_y_gen2(dt, _x0, error_model, sensor_pt);
    }
    if(Hx != 0) { 
        SurviveJointKalmanErrorModel_LightMeas_y_gen2_jac_sensor_pt(Hx, dt, _x0, error_model, sensor_pt);
    }
}
