Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcP.cpp

557 lines
20 KiB
C++
Raw Blame History

float t2 = dax*0.5f;
float t3 = dax_b*0.5f;
float t4 = t2-t3;
float t5 = day*0.5f;
float t6 = day_b*0.5f;
float t7 = t5-t6;
float t8 = daz*0.5f;
float t9 = daz_b*0.5f;
float t10 = t8-t9;
float t11 = q2*t4*0.5f;
float t12 = q1*t7*0.5f;
float t13 = q0*t10*0.5f;
float t14 = q2*0.5f;
float t15 = q3*t4*0.5f;
float t16 = q1*t10*0.5f;
float t17 = q1*0.5f;
float t18 = q0*t4*0.5f;
float t19 = q3*t7*0.5f;
float t20 = q0*0.5f;
float t21 = q2*t7*0.5f;
float t22 = q3*t10*0.5f;
float t23 = q0*t7*0.5f;
float t24 = q3*0.5f;
float t25 = q1*t4*0.5f;
float t26 = q2*t10*0.5f;
float t27 = t11+t12+t13-t24;
float t28 = t14+t15+t16-t23;
float t29 = q2*t28*2.0f;
float t30 = t17+t18+t19-t26;
float t31 = q1*t30*2.0f;
float t32 = t20+t21+t22-t25;
float t33 = q0*t32*2.0f;
float t40 = q3*t27*2.0f;
float t34 = t29+t31+t33-t40;
float t35 = q0*q0;
float t36 = q1*q1;
float t37 = q2*q2;
float t38 = q3*q3;
float t39 = t35+t36+t37+t38;
float t41 = t11+t12-t13+t24;
float t42 = t14-t15+t16+t23;
float t43 = q1*t42*2.0f;
float t44 = -t17+t18+t19+t26;
float t45 = q2*t44*2.0f;
float t46 = t20-t21+t22+t25;
float t47 = q3*t46*2.0f;
float t57 = q0*t41*2.0f;
float t48 = t43+t45+t47-t57;
float t49 = -t14+t15+t16+t23;
float t50 = q0*t49*2.0f;
float t51 = t11-t12+t13+t24;
float t52 = t20+t21-t22+t25;
float t53 = q2*t52*2.0f;
float t54 = t17-t18+t19+t26;
float t55 = q3*t54*2.0f;
float t58 = q1*t51*2.0f;
float t56 = t50+t53+t55-t58;
float t59 = OP[0][0]*t34;
float t60 = OP[1][0]*t48;
float t66 = OP[6][0]*t39;
float t67 = OP[2][0]*t56;
float t61 = t59+t60-t66-t67;
float t62 = OP[0][1]*t34;
float t63 = OP[1][1]*t48;
float t64 = OP[0][2]*t34;
float t65 = OP[1][2]*t48;
float t72 = OP[6][1]*t39;
float t73 = OP[2][1]*t56;
float t68 = t62+t63-t72-t73;
float t69 = t35+t36-t37-t38;
float t86 = OP[6][2]*t39;
float t87 = OP[2][2]*t56;
float t70 = t64+t65-t86-t87;
float t71 = dvx-dvx_b;
float t74 = q0*q3*2.0f;
float t81 = q1*q2*2.0f;
float t75 = t74-t81;
float t76 = q0*q2*2.0f;
float t77 = q1*q3*2.0f;
float t78 = t76+t77;
float t79 = dvy-dvy_b;
float t80 = dvz-dvz_b;
float t82 = OP[0][10]*t34;
float t83 = OP[1][10]*t48;
float t103 = OP[6][10]*t39;
float t104 = OP[2][10]*t56;
float t84 = t82+t83-t103-t104;
float t85 = t35-t36+t37-t38;
float t88 = t74+t81;
float t89 = OP[0][9]*t34;
float t90 = OP[1][9]*t48;
float t100 = OP[6][9]*t39;
float t101 = OP[2][9]*t56;
float t91 = t89+t90-t100-t101;
float t92 = q0*q1*2.0f;
float t96 = q2*q3*2.0f;
float t93 = t92-t96;
float t94 = OP[0][11]*t34;
float t95 = OP[1][11]*t48;
float t114 = OP[6][11]*t39;
float t115 = OP[2][11]*t56;
float t97 = t94+t95-t114-t115;
float t98 = t35-t36-t37+t38;
float t99 = t76-t77;
float t102 = t92+t96;
float t105 = OP[0][6]*t34;
float t106 = OP[1][6]*t48;
float t411 = OP[6][6]*t39;
float t107 = t105+t106-t411-OP[2][6]*t56;
float t108 = OP[0][7]*t34;
float t109 = OP[1][7]*t48;
float t412 = OP[6][7]*t39;
float t110 = t108+t109-t412-OP[2][7]*t56;
float t111 = OP[0][8]*t34;
float t112 = OP[1][8]*t48;
float t413 = OP[6][8]*t39;
float t113 = t111+t112-t413-OP[2][8]*t56;
float t116 = q0*t27*2.0f;
float t117 = q1*t28*2.0f;
float t118 = q3*t32*2.0f;
float t128 = q2*t30*2.0f;
float t119 = t116+t117+t118-t128;
float t120 = q0*t46*2.0f;
float t121 = q2*t42*2.0f;
float t122 = q3*t41*2.0f;
float t129 = q1*t44*2.0f;
float t123 = t120+t121+t122-t129;
float t124 = q1*t52*2.0f;
float t125 = q2*t51*2.0f;
float t126 = q3*t49*2.0f;
float t130 = q0*t54*2.0f;
float t127 = t124+t125+t126-t130;
float t131 = OP[7][0]*t39;
float t132 = OP[0][0]*t119;
float t141 = OP[1][0]*t123;
float t142 = OP[2][0]*t127;
float t133 = t131+t132-t141-t142;
float t134 = OP[7][1]*t39;
float t135 = OP[0][1]*t119;
float t147 = OP[1][1]*t123;
float t148 = OP[2][1]*t127;
float t136 = t134+t135-t147-t148;
float t137 = OP[7][2]*t39;
float t138 = OP[0][2]*t119;
float t153 = OP[1][2]*t123;
float t154 = OP[2][2]*t127;
float t139 = t137+t138-t153-t154;
float t140 = t39*t39;
float t143 = q1*t27*2.0f;
float t144 = q2*t32*2.0f;
float t145 = q3*t30*2.0f;
float t204 = q0*t28*2.0f;
float t146 = t143+t144+t145-t204;
float t149 = q0*t44*2.0f;
float t150 = q1*t46*2.0f;
float t151 = q2*t41*2.0f;
float t205 = q3*t42*2.0f;
float t152 = t149+t150+t151-t205;
float t155 = q0*t52*2.0f;
float t156 = q1*t54*2.0f;
float t157 = q3*t51*2.0f;
float t206 = q2*t49*2.0f;
float t158 = t155+t156+t157-t206;
float t159 = t69*t79;
float t160 = t71*t75;
float t161 = t159+t160;
float t162 = t69*t80;
float t222 = t71*t78;
float t163 = t162-t222;
float t164 = t78*t79;
float t165 = t75*t80;
float t166 = t164+t165;
float t167 = OP[7][10]*t39;
float t168 = OP[0][10]*t119;
float t193 = OP[1][10]*t123;
float t194 = OP[2][10]*t127;
float t169 = t167+t168-t193-t194;
float t170 = t71*t85;
float t226 = t79*t88;
float t171 = t170-t226;
float t172 = t80*t85;
float t173 = t79*t93;
float t174 = t172+t173;
float t175 = OP[7][9]*t39;
float t176 = OP[0][9]*t119;
float t191 = OP[1][9]*t123;
float t192 = OP[2][9]*t127;
float t177 = t175+t176-t191-t192;
float t178 = OP[7][11]*t39;
float t179 = OP[0][11]*t119;
float t184 = OP[1][11]*t123;
float t185 = OP[2][11]*t127;
float t180 = t178+t179-t184-t185;
float t181 = t71*t93;
float t182 = t80*t88;
float t183 = t181+t182;
float t186 = t71*t98;
float t187 = t80*t99;
float t188 = t186+t187;
float t189 = t79*t98;
float t233 = t80*t102;
float t190 = t189-t233;
float t195 = t71*t102;
float t196 = t79*t99;
float t197 = t195+t196;
float t198 = OP[7][6]*t39;
float t199 = OP[0][6]*t119;
float t200 = OP[7][7]*t39;
float t201 = OP[0][7]*t119;
float t202 = OP[7][8]*t39;
float t203 = OP[0][8]*t119;
float t207 = OP[8][0]*t39;
float t208 = OP[1][0]*t152;
float t216 = OP[0][0]*t146;
float t217 = OP[2][0]*t158;
float t209 = t207+t208-t216-t217;
float t210 = OP[8][1]*t39;
float t211 = OP[1][1]*t152;
float t218 = OP[0][1]*t146;
float t219 = OP[2][1]*t158;
float t212 = t210+t211-t218-t219;
float t213 = OP[8][2]*t39;
float t214 = OP[1][2]*t152;
float t220 = OP[0][2]*t146;
float t221 = OP[2][2]*t158;
float t215 = t213+t214-t220-t221;
float t223 = OP[0][10]*t146;
float t224 = OP[2][10]*t158;
float t236 = OP[8][10]*t39;
float t237 = OP[1][10]*t152;
float t225 = t223+t224-t236-t237;
float t227 = OP[0][9]*t146;
float t228 = OP[2][9]*t158;
float t234 = OP[8][9]*t39;
float t235 = OP[1][9]*t152;
float t229 = t227+t228-t234-t235;
float t230 = OP[0][11]*t146;
float t231 = OP[2][11]*t158;
float t244 = OP[8][11]*t39;
float t245 = OP[1][11]*t152;
float t232 = t230+t231-t244-t245;
float t238 = OP[8][6]*t39;
float t239 = OP[1][6]*t152;
float t240 = OP[8][7]*t39;
float t241 = OP[1][7]*t152;
float t242 = OP[8][8]*t39;
float t243 = OP[1][8]*t152;
float t246 = OP[1][0]*t163;
float t247 = OP[10][0]*t75;
float t248 = OP[0][0]*t166;
float t258 = OP[9][0]*t69;
float t259 = OP[2][0]*t161;
float t260 = OP[11][0]*t78;
float t249 = OP[3][0]+t246+t247+t248-t258-t259-t260;
float t250 = OP[1][1]*t163;
float t251 = OP[10][1]*t75;
float t252 = OP[0][1]*t166;
float t261 = OP[9][1]*t69;
float t262 = OP[2][1]*t161;
float t263 = OP[11][1]*t78;
float t253 = OP[3][1]+t250+t251+t252-t261-t262-t263;
float t254 = OP[1][2]*t163;
float t255 = OP[10][2]*t75;
float t256 = OP[0][2]*t166;
float t264 = OP[9][2]*t69;
float t265 = OP[2][2]*t161;
float t266 = OP[11][2]*t78;
float t257 = OP[3][2]+t254+t255+t256-t264-t265-t266;
float t267 = OP[1][9]*t163;
float t268 = OP[10][9]*t75;
float t269 = OP[0][9]*t166;
float t279 = OP[9][9]*t69;
float t280 = OP[2][9]*t161;
float t281 = OP[11][9]*t78;
float t270 = OP[3][9]+t267+t268+t269-t279-t280-t281;
float t271 = OP[1][11]*t163;
float t272 = OP[10][11]*t75;
float t273 = OP[0][11]*t166;
float t285 = OP[9][11]*t69;
float t286 = OP[2][11]*t161;
float t287 = OP[11][11]*t78;
float t274 = OP[3][11]+t271+t272+t273-t285-t286-t287;
float t275 = OP[1][10]*t163;
float t276 = OP[10][10]*t75;
float t277 = OP[0][10]*t166;
float t282 = OP[9][10]*t69;
float t283 = OP[2][10]*t161;
float t284 = OP[11][10]*t78;
float t278 = OP[3][10]+t275+t276+t277-t282-t283-t284;
float t288 = OP[1][6]*t163;
float t289 = OP[10][6]*t75;
float t290 = OP[0][6]*t166;
float t291 = OP[3][6]+t288+t289+t290-OP[9][6]*t69-OP[2][6]*t161-OP[11][6]*t78;
float t292 = OP[1][7]*t163;
float t293 = OP[10][7]*t75;
float t294 = OP[0][7]*t166;
float t295 = OP[3][7]+t292+t293+t294-OP[9][7]*t69-OP[2][7]*t161-OP[11][7]*t78;
float t296 = OP[1][8]*t163;
float t297 = OP[10][8]*t75;
float t298 = OP[0][8]*t166;
float t299 = OP[3][8]+t296+t297+t298-OP[9][8]*t69-OP[2][8]*t161-OP[11][8]*t78;
float t300 = OP[2][0]*t171;
float t301 = OP[11][0]*t93;
float t302 = OP[1][0]*t183;
float t312 = OP[10][0]*t85;
float t313 = OP[0][0]*t174;
float t314 = OP[9][0]*t88;
float t303 = OP[4][0]+t300+t301+t302-t312-t313-t314;
float t304 = OP[2][1]*t171;
float t305 = OP[11][1]*t93;
float t306 = OP[1][1]*t183;
float t315 = OP[10][1]*t85;
float t316 = OP[0][1]*t174;
float t317 = OP[9][1]*t88;
float t307 = OP[4][1]+t304+t305+t306-t315-t316-t317;
float t308 = OP[2][2]*t171;
float t309 = OP[11][2]*t93;
float t310 = OP[1][2]*t183;
float t318 = OP[10][2]*t85;
float t319 = OP[0][2]*t174;
float t320 = OP[9][2]*t88;
float t311 = OP[4][2]+t308+t309+t310-t318-t319-t320;
float t321 = dvxNoise*t69*t88;
float t322 = OP[2][9]*t171;
float t323 = OP[11][9]*t93;
float t324 = OP[1][9]*t183;
float t334 = OP[10][9]*t85;
float t335 = OP[0][9]*t174;
float t336 = OP[9][9]*t88;
float t325 = OP[4][9]+t322+t323+t324-t334-t335-t336;
float t326 = OP[2][11]*t171;
float t327 = OP[11][11]*t93;
float t328 = OP[1][11]*t183;
float t340 = OP[10][11]*t85;
float t341 = OP[0][11]*t174;
float t342 = OP[9][11]*t88;
float t329 = OP[4][11]+t326+t327+t328-t340-t341-t342;
float t330 = OP[2][10]*t171;
float t331 = OP[11][10]*t93;
float t332 = OP[1][10]*t183;
float t337 = OP[10][10]*t85;
float t338 = OP[0][10]*t174;
float t339 = OP[9][10]*t88;
float t333 = OP[4][10]+t330+t331+t332-t337-t338-t339;
float t343 = OP[2][6]*t171;
float t344 = OP[11][6]*t93;
float t345 = OP[1][6]*t183;
float t346 = OP[4][6]+t343+t344+t345-OP[0][6]*t174-OP[9][6]*t88-OP[10][6]*t85;
float t347 = OP[2][7]*t171;
float t348 = OP[11][7]*t93;
float t349 = OP[1][7]*t183;
float t350 = OP[4][7]+t347+t348+t349-OP[0][7]*t174-OP[9][7]*t88-OP[10][7]*t85;
float t351 = OP[2][8]*t171;
float t352 = OP[11][8]*t93;
float t353 = OP[1][8]*t183;
float t354 = OP[4][8]+t351+t352+t353-OP[0][8]*t174-OP[9][8]*t88-OP[10][8]*t85;
float t355 = OP[0][0]*t190;
float t356 = OP[9][0]*t99;
float t357 = OP[2][0]*t197;
float t367 = OP[11][0]*t98;
float t368 = OP[1][0]*t188;
float t369 = OP[10][0]*t102;
float t358 = OP[5][0]+t355+t356+t357-t367-t368-t369;
float t359 = OP[0][1]*t190;
float t360 = OP[9][1]*t99;
float t361 = OP[2][1]*t197;
float t370 = OP[11][1]*t98;
float t371 = OP[1][1]*t188;
float t372 = OP[10][1]*t102;
float t362 = OP[5][1]+t359+t360+t361-t370-t371-t372;
float t363 = OP[0][2]*t190;
float t364 = OP[9][2]*t99;
float t365 = OP[2][2]*t197;
float t373 = OP[11][2]*t98;
float t374 = OP[1][2]*t188;
float t375 = OP[10][2]*t102;
float t366 = OP[5][2]+t363+t364+t365-t373-t374-t375;
float t376 = dvzNoise*t78*t98;
float t377 = OP[0][9]*t190;
float t378 = OP[9][9]*t99;
float t379 = OP[2][9]*t197;
float t390 = OP[11][9]*t98;
float t391 = OP[1][9]*t188;
float t392 = OP[10][9]*t102;
float t380 = OP[5][9]+t377+t378+t379-t390-t391-t392;
float t381 = OP[0][11]*t190;
float t382 = OP[9][11]*t99;
float t383 = OP[2][11]*t197;
float t396 = OP[11][11]*t98;
float t397 = OP[1][11]*t188;
float t398 = OP[10][11]*t102;
float t384 = OP[5][11]+t381+t382+t383-t396-t397-t398;
float t385 = OP[0][10]*t190;
float t386 = OP[9][10]*t99;
float t387 = OP[2][10]*t197;
float t393 = OP[11][10]*t98;
float t394 = OP[1][10]*t188;
float t395 = OP[10][10]*t102;
float t388 = OP[5][10]+t385+t386+t387-t393-t394-t395;
float t389 = dvyNoise*t85*t102;
float t399 = OP[0][6]*t190;
float t400 = OP[9][6]*t99;
float t401 = OP[2][6]*t197;
float t402 = OP[5][6]+t399+t400+t401-OP[1][6]*t188-OP[10][6]*t102-OP[11][6]*t98;
float t403 = OP[0][7]*t190;
float t404 = OP[9][7]*t99;
float t405 = OP[2][7]*t197;
float t406 = OP[5][7]+t403+t404+t405-OP[1][7]*t188-OP[10][7]*t102-OP[11][7]*t98;
float t407 = OP[0][8]*t190;
float t408 = OP[9][8]*t99;
float t409 = OP[2][8]*t197;
float t410 = OP[5][8]+t407+t408+t409-OP[1][8]*t188-OP[10][8]*t102-OP[11][8]*t98;
A0[0][0] = daxNoise*t140+t34*t61+t48*t68-t56*t70-t39*t107;
A0[0][1] = -t39*t110-t61*t119+t68*t123+t70*t127;
A0[0][2] = -t39*t113-t68*t152+t70*t158+t146*(t59+t60-t66-t67);
A0[0][3] = OP[0][3]*t34+OP[1][3]*t48-OP[2][3]*t56-OP[6][3]*t39+t75*t84-t69*t91-t78*t97-t70*t161+t166*(t59+t60-t66-t67)+t163*(t62+t63-t72-t73);
A0[0][4] = OP[0][4]*t34+OP[1][4]*t48-OP[2][4]*t56-OP[6][4]*t39-t84*t85-t88*t91+t93*t97-t61*t174+t70*t171+t183*(t62+t63-t72-t73);
A0[0][5] = OP[0][5]*t34+OP[1][5]*t48-OP[2][5]*t56-OP[6][5]*t39-t84*t102-t97*t98+t61*t190-t68*t188+t99*(t89+t90-t100-t101)+t197*(t64+t65-t86-t87);
A0[0][6] = t107;
A0[0][7] = t110;
A0[0][8] = t113;
A0[0][9] = t91;
A0[0][10] = t84;
A0[0][11] = t97;
A0[1][0] = -t34*t133-t48*t136+t56*t139+t39*(t198+t199-OP[1][6]*t123-OP[2][6]*t127);
A0[1][1] = dayNoise*t140+t119*t133-t123*t136-t127*t139+t39*(t200+t201-OP[1][7]*t123-OP[2][7]*t127);
A0[1][2] = -t133*t146+t136*t152-t139*t158+t39*(t202+t203-OP[1][8]*t123-OP[2][8]*t127);
A0[1][3] = -OP[7][3]*t39-OP[0][3]*t119+OP[1][3]*t123+OP[2][3]*t127-t75*t169+t69*t177+t78*t180-t133*t166-t136*t163+t139*t161;
A0[1][4] = -OP[7][4]*t39-OP[0][4]*t119+OP[1][4]*t123+OP[2][4]*t127+t85*t169+t88*t177-t93*t180+t133*t174-t139*t171-t136*t183;
A0[1][5] = -OP[7][5]*t39-OP[0][5]*t119+OP[1][5]*t123+OP[2][5]*t127+t102*t169-t99*t177+t98*t180-t133*t190+t136*t188-t139*t197;
A0[1][6] = -t198-t199+OP[1][6]*t123+OP[2][6]*t127;
A0[1][7] = -t200-t201+OP[1][7]*t123+OP[2][7]*t127;
A0[1][8] = -t202-t203+OP[1][8]*t123+OP[2][8]*t127;
A0[1][9] = -t175-t176+t191+t192;
A0[1][10] = -t167-t168+t193+t194;
A0[1][11] = -t178-t179+t184+t185;
A0[2][0] = -t34*t209-t48*t212+t56*t215+t39*(t238+t239-OP[0][6]*t146-OP[2][6]*t158);
A0[2][1] = t119*t209-t123*t212-t127*t215+t39*(t240+t241-OP[0][7]*t146-OP[2][7]*t158);
A0[2][2] = dazNoise*t140-t146*t209+t152*t212-t158*t215+t39*(t242+t243-OP[0][8]*t146-OP[2][8]*t158);
A0[2][3] = -OP[8][3]*t39+OP[0][3]*t146-OP[1][3]*t152+OP[2][3]*t158-t69*t229+t75*t225-t78*t232-t163*t212-t166*t209+t161*t215;
A0[2][4] = -OP[8][4]*t39+OP[0][4]*t146-OP[1][4]*t152+OP[2][4]*t158-t85*t225-t88*t229+t93*t232+t174*t209-t171*t215-t183*t212;
A0[2][5] = -OP[8][5]*t39+OP[0][5]*t146-OP[1][5]*t152+OP[2][5]*t158-t102*t225-t98*t232-t190*t209+t188*t212-t197*t215+t99*(t227+t228-t234-t235);
A0[2][6] = -t238-t239+OP[0][6]*t146+OP[2][6]*t158;
A0[2][7] = -t240-t241+OP[0][7]*t146+OP[2][7]*t158;
A0[2][8] = -t242-t243+OP[0][8]*t146+OP[2][8]*t158;
A0[2][9] = t229;
A0[2][10] = t225;
A0[2][11] = t232;
A0[3][0] = t34*t249+t48*t253-t56*t257-t39*t291;
A0[3][1] = -t39*t295-t119*t249+t123*t253+t127*t257;
A0[3][2] = -t39*t299+t146*t249-t152*t253+t158*t257;
A0[3][3] = OP[3][3]-OP[9][3]*t69+OP[0][3]*t166+OP[1][3]*t163+OP[10][3]*t75-OP[2][3]*t161-OP[11][3]*t78-t69*t270-t78*t274+t75*t278+t166*t249+t163*t253-t161*t257+dvxNoise*(t69*t69)+dvyNoise*(t75*t75)+dvzNoise*(t78*t78);
A0[3][4] = OP[3][4]+t321-OP[9][4]*t69+OP[0][4]*t166+OP[1][4]*t163+OP[10][4]*t75-OP[2][4]*t161-OP[11][4]*t78-t88*t270-t85*t278+t93*t274-t174*t249+t171*t257+t183*t253-dvyNoise*t75*t85-dvzNoise*t78*t93;
A0[3][5] = OP[3][5]+t376-OP[9][5]*t69+OP[0][5]*t166+OP[1][5]*t163+OP[10][5]*t75-OP[2][5]*t161-OP[11][5]*t78+t99*t270-t98*t274-t102*t278+t190*t249-t188*t253+t197*t257-dvxNoise*t69*t99-dvyNoise*t75*t102;
A0[3][6] = t291;
A0[3][7] = t295;
A0[3][8] = t299;
A0[3][9] = t270;
A0[3][10] = t278;
A0[3][11] = t274;
A0[4][0] = t34*t303+t48*t307-t56*t311-t39*t346;
A0[4][1] = -t39*t350-t119*t303+t123*t307+t127*t311;
A0[4][2] = -t39*t354+t146*t303-t152*t307+t158*t311;
A0[4][3] = OP[4][3]+t321-OP[0][3]*t174-OP[9][3]*t88-OP[10][3]*t85+OP[2][3]*t171+OP[1][3]*t183+OP[11][3]*t93-t69*t325-t78*t329+t75*t333+t166*t303+t163*t307-t161*t311-dvyNoise*t75*t85-dvzNoise*t78*t93;
A0[4][4] = OP[4][4]-OP[0][4]*t174-OP[9][4]*t88-OP[10][4]*t85+OP[2][4]*t171+OP[1][4]*t183+OP[11][4]*t93-t88*t325-t85*t333+t93*t329-t174*t303+t171*t311+t183*t307+dvxNoise*(t88*t88)+dvyNoise*(t85*t85)+dvzNoise*(t93*t93);
A0[4][5] = OP[4][5]+t389-OP[0][5]*t174-OP[9][5]*t88-OP[10][5]*t85+OP[2][5]*t171+OP[1][5]*t183+OP[11][5]*t93+t99*t325-t98*t329-t102*t333+t190*t303-t188*t307+t197*t311-dvxNoise*t88*t99-dvzNoise*t93*t98;
A0[4][6] = t346;
A0[4][7] = t350;
A0[4][8] = t354;
A0[4][9] = t325;
A0[4][10] = t333;
A0[4][11] = t329;
A0[5][0] = t34*t358+t48*t362-t56*t366-t39*t402;
A0[5][1] = -t39*t406-t119*t358+t123*t362+t127*t366;
A0[5][2] = -t39*t410+t146*t358-t152*t362+t158*t366;
A0[5][3] = OP[5][3]+t376+OP[9][3]*t99+OP[0][3]*t190-OP[1][3]*t188-OP[10][3]*t102-OP[11][3]*t98+OP[2][3]*t197-t69*t380-t78*t384+t75*t388+t166*t358+t163*t362-t161*t366-dvxNoise*t69*t99-dvyNoise*t75*t102;
A0[5][4] = OP[5][4]+t389+OP[9][4]*t99+OP[0][4]*t190-OP[1][4]*t188-OP[10][4]*t102-OP[11][4]*t98+OP[2][4]*t197-t88*t380-t85*t388+t93*t384-t174*t358+t171*t366+t183*t362-dvxNoise*t88*t99-dvzNoise*t93*t98;
A0[5][5] = OP[5][5]+OP[9][5]*t99+OP[0][5]*t190-OP[1][5]*t188-OP[10][5]*t102-OP[11][5]*t98+OP[2][5]*t197+t99*t380-t98*t384-t102*t388+t190*t358-t188*t362+t197*t366+dvxNoise*(t99*t99)+dvyNoise*(t102*t102)+dvzNoise*(t98*t98);
A0[5][6] = t402;
A0[5][7] = t406;
A0[5][8] = t410;
A0[5][9] = t380;
A0[5][10] = t388;
A0[5][11] = t384;
A0[6][0] = -t411+OP[6][0]*t34+OP[6][1]*t48-OP[6][2]*t56;
A0[6][1] = -t412-OP[6][0]*t119+OP[6][1]*t123+OP[6][2]*t127;
A0[6][2] = -t413+OP[6][0]*t146-OP[6][1]*t152+OP[6][2]*t158;
A0[6][3] = OP[6][3]-OP[6][2]*t161+OP[6][1]*t163+OP[6][0]*t166-OP[6][9]*t69+OP[6][10]*t75-OP[6][11]*t78;
A0[6][4] = OP[6][4]+OP[6][2]*t171-OP[6][0]*t174+OP[6][1]*t183-OP[6][10]*t85-OP[6][9]*t88+OP[6][11]*t93;
A0[6][5] = OP[6][5]-OP[6][1]*t188+OP[6][0]*t190+OP[6][2]*t197+OP[6][9]*t99-OP[6][11]*t98-OP[6][10]*t102;
A0[6][6] = OP[6][6];
A0[6][7] = OP[6][7];
A0[6][8] = OP[6][8];
A0[6][9] = OP[6][9];
A0[6][10] = OP[6][10];
A0[6][11] = OP[6][11];
A0[7][0] = -t198+OP[7][0]*t34+OP[7][1]*t48-OP[7][2]*t56;
A0[7][1] = -t200-OP[7][0]*t119+OP[7][1]*t123+OP[7][2]*t127;
A0[7][2] = -t202+OP[7][0]*t146-OP[7][1]*t152+OP[7][2]*t158;
A0[7][3] = OP[7][3]-OP[7][2]*t161+OP[7][1]*t163+OP[7][0]*t166-OP[7][9]*t69+OP[7][10]*t75-OP[7][11]*t78;
A0[7][4] = OP[7][4]+OP[7][2]*t171-OP[7][0]*t174+OP[7][1]*t183-OP[7][10]*t85-OP[7][9]*t88+OP[7][11]*t93;
A0[7][5] = OP[7][5]-OP[7][1]*t188+OP[7][0]*t190+OP[7][2]*t197+OP[7][9]*t99-OP[7][11]*t98-OP[7][10]*t102;
A0[7][6] = OP[7][6];
A0[7][7] = OP[7][7];
A0[7][8] = OP[7][8];
A0[7][9] = OP[7][9];
A0[7][10] = OP[7][10];
A0[7][11] = OP[7][11];
A0[8][0] = -t238+OP[8][0]*t34+OP[8][1]*t48-OP[8][2]*t56;
A0[8][1] = -t240-OP[8][0]*t119+OP[8][1]*t123+OP[8][2]*t127;
A0[8][2] = -t242+OP[8][0]*t146-OP[8][1]*t152+OP[8][2]*t158;
A0[8][3] = OP[8][3]-OP[8][2]*t161+OP[8][1]*t163+OP[8][0]*t166-OP[8][9]*t69+OP[8][10]*t75-OP[8][11]*t78;
A0[8][4] = OP[8][4]+OP[8][2]*t171-OP[8][0]*t174+OP[8][1]*t183-OP[8][10]*t85-OP[8][9]*t88+OP[8][11]*t93;
A0[8][5] = OP[8][5]-OP[8][1]*t188+OP[8][0]*t190+OP[8][2]*t197+OP[8][9]*t99-OP[8][11]*t98-OP[8][10]*t102;
A0[8][6] = OP[8][6];
A0[8][7] = OP[8][7];
A0[8][8] = OP[8][8];
A0[8][9] = OP[8][9];
A0[8][10] = OP[8][10];
A0[8][11] = OP[8][11];
A0[9][0] = OP[9][0]*t34-OP[9][6]*t39+OP[9][1]*t48-OP[9][2]*t56;
A0[9][1] = -OP[9][7]*t39-OP[9][0]*t119+OP[9][1]*t123+OP[9][2]*t127;
A0[9][2] = -OP[9][8]*t39+OP[9][0]*t146-OP[9][1]*t152+OP[9][2]*t158;
A0[9][3] = OP[9][3]-t279-OP[9][2]*t161+OP[9][1]*t163+OP[9][0]*t166+OP[9][10]*t75-OP[9][11]*t78;
A0[9][4] = OP[9][4]-t336+OP[9][2]*t171-OP[9][0]*t174+OP[9][1]*t183-OP[9][10]*t85+OP[9][11]*t93;
A0[9][5] = OP[9][5]+t378-OP[9][1]*t188+OP[9][0]*t190+OP[9][2]*t197-OP[9][11]*t98-OP[9][10]*t102;
A0[9][6] = OP[9][6];
A0[9][7] = OP[9][7];
A0[9][8] = OP[9][8];
A0[9][9] = OP[9][9];
A0[9][10] = OP[9][10];
A0[9][11] = OP[9][11];
A0[10][0] = OP[10][0]*t34-OP[10][6]*t39+OP[10][1]*t48-OP[10][2]*t56;
A0[10][1] = -OP[10][7]*t39-OP[10][0]*t119+OP[10][1]*t123+OP[10][2]*t127;
A0[10][2] = -OP[10][8]*t39+OP[10][0]*t146-OP[10][1]*t152+OP[10][2]*t158;
A0[10][3] = OP[10][3]+t276-OP[10][2]*t161+OP[10][1]*t163+OP[10][0]*t166-OP[10][9]*t69-OP[10][11]*t78;
A0[10][4] = OP[10][4]-t337+OP[10][2]*t171-OP[10][0]*t174+OP[10][1]*t183-OP[10][9]*t88+OP[10][11]*t93;
A0[10][5] = OP[10][5]-t395-OP[10][1]*t188+OP[10][0]*t190+OP[10][2]*t197+OP[10][9]*t99-OP[10][11]*t98;
A0[10][6] = OP[10][6];
A0[10][7] = OP[10][7];
A0[10][8] = OP[10][8];
A0[10][9] = OP[10][9];
A0[10][10] = OP[10][10];
A0[10][11] = OP[10][11];
A0[11][0] = OP[11][0]*t34-OP[11][6]*t39+OP[11][1]*t48-OP[11][2]*t56;
A0[11][1] = -OP[11][7]*t39-OP[11][0]*t119+OP[11][1]*t123+OP[11][2]*t127;
A0[11][2] = -OP[11][8]*t39+OP[11][0]*t146-OP[11][1]*t152+OP[11][2]*t158;
A0[11][3] = OP[11][3]-t287-OP[11][2]*t161+OP[11][1]*t163+OP[11][0]*t166-OP[11][9]*t69+OP[11][10]*t75;
A0[11][4] = OP[11][4]+t327+OP[11][2]*t171-OP[11][0]*t174+OP[11][1]*t183-OP[11][10]*t85-OP[11][9]*t88;
A0[11][5] = OP[11][5]-t396-OP[11][1]*t188+OP[11][0]*t190+OP[11][2]*t197+OP[11][9]*t99-OP[11][10]*t102;
A0[11][6] = OP[11][6];
A0[11][7] = OP[11][7];
A0[11][8] = OP[11][8];
A0[11][9] = OP[11][9];
A0[11][10] = OP[11][10];
A0[11][11] = OP[11][11];
Reference in New Issue View Git Blame Copy Permalink