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.c

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

557 lines
22 KiB
C
Raw Normal View History

AP_NavEKF: Add Matlab derivations and simulations behind small EKF
2015-04-01 17:54:15 -03:00
t2 = dax*(1.0/2.0);
t3 = dax_b*(1.0/2.0);
t4 = t2-t3;
t5 = day*(1.0/2.0);
t6 = day_b*(1.0/2.0);
t7 = t5-t6;
t8 = daz*(1.0/2.0);
t9 = daz_b*(1.0/2.0);
t10 = t8-t9;
t11 = q2*t4*(1.0/2.0);
t12 = q1*t7*(1.0/2.0);
t13 = q0*t10*(1.0/2.0);
t14 = q2*(1.0/2.0);
t15 = q3*t4*(1.0/2.0);
t16 = q1*t10*(1.0/2.0);
t17 = q1*(1.0/2.0);
t18 = q0*t4*(1.0/2.0);
t19 = q3*t7*(1.0/2.0);
t20 = q0*(1.0/2.0);
t21 = q2*t7*(1.0/2.0);
t22 = q3*t10*(1.0/2.0);
t23 = q0*t7*(1.0/2.0);
t24 = q3*(1.0/2.0);
t25 = q1*t4*(1.0/2.0);
t26 = q2*t10*(1.0/2.0);
t27 = t11+t12+t13-t24;
t28 = t14+t15+t16-t23;
t29 = q2*t28*2.0;
t30 = t17+t18+t19-t26;
t31 = q1*t30*2.0;
t32 = t20+t21+t22-t25;
t33 = q0*t32*2.0;
t40 = q3*t27*2.0;
t34 = t29+t31+t33-t40;
t35 = q0*q0;
t36 = q1*q1;
t37 = q2*q2;
t38 = q3*q3;
t39 = t35+t36+t37+t38;
t41 = t11+t12-t13+t24;
t42 = t14-t15+t16+t23;
t43 = q1*t42*2.0;
t44 = -t17+t18+t19+t26;
t45 = q2*t44*2.0;
t46 = t20-t21+t22+t25;
t47 = q3*t46*2.0;
t57 = q0*t41*2.0;
t48 = t43+t45+t47-t57;
t49 = -t14+t15+t16+t23;
t50 = q0*t49*2.0;
t51 = t11-t12+t13+t24;
t52 = t20+t21-t22+t25;
t53 = q2*t52*2.0;
t54 = t17-t18+t19+t26;
t55 = q3*t54*2.0;
t58 = q1*t51*2.0;
t56 = t50+t53+t55-t58;
t59 = OP_l_1_c_1_r_*t34;
t60 = OP_l_2_c_1_r_*t48;
t66 = OP_l_7_c_1_r_*t39;
t67 = OP_l_3_c_1_r_*t56;
t61 = t59+t60-t66-t67;
t62 = OP_l_1_c_2_r_*t34;
t63 = OP_l_2_c_2_r_*t48;
t64 = OP_l_1_c_3_r_*t34;
t65 = OP_l_2_c_3_r_*t48;
t72 = OP_l_7_c_2_r_*t39;
t73 = OP_l_3_c_2_r_*t56;
t68 = t62+t63-t72-t73;
t69 = t35+t36-t37-t38;
t86 = OP_l_7_c_3_r_*t39;
t87 = OP_l_3_c_3_r_*t56;
t70 = t64+t65-t86-t87;
t71 = dvx-dvx_b;
t74 = q0*q3*2.0;
t81 = q1*q2*2.0;
t75 = t74-t81;
t76 = q0*q2*2.0;
t77 = q1*q3*2.0;
t78 = t76+t77;
t79 = dvy-dvy_b;
t80 = dvz-dvz_b;
t82 = OP_l_1_c_11_r_*t34;
t83 = OP_l_2_c_11_r_*t48;
t103 = OP_l_7_c_11_r_*t39;
t104 = OP_l_3_c_11_r_*t56;
t84 = t82+t83-t103-t104;
t85 = t35-t36+t37-t38;
t88 = t74+t81;
t89 = OP_l_1_c_10_r_*t34;
t90 = OP_l_2_c_10_r_*t48;
t100 = OP_l_7_c_10_r_*t39;
t101 = OP_l_3_c_10_r_*t56;
t91 = t89+t90-t100-t101;
t92 = q0*q1*2.0;
t96 = q2*q3*2.0;
t93 = t92-t96;
t94 = OP_l_1_c_12_r_*t34;
t95 = OP_l_2_c_12_r_*t48;
t114 = OP_l_7_c_12_r_*t39;
t115 = OP_l_3_c_12_r_*t56;
t97 = t94+t95-t114-t115;
t98 = t35-t36-t37+t38;
t99 = t76-t77;
t102 = t92+t96;
t105 = OP_l_1_c_7_r_*t34;
t106 = OP_l_2_c_7_r_*t48;
t411 = OP_l_7_c_7_r_*t39;
t107 = t105+t106-t411-OP_l_3_c_7_r_*t56;
t108 = OP_l_1_c_8_r_*t34;
t109 = OP_l_2_c_8_r_*t48;
t412 = OP_l_7_c_8_r_*t39;
t110 = t108+t109-t412-OP_l_3_c_8_r_*t56;
t111 = OP_l_1_c_9_r_*t34;
t112 = OP_l_2_c_9_r_*t48;
t413 = OP_l_7_c_9_r_*t39;
t113 = t111+t112-t413-OP_l_3_c_9_r_*t56;
t116 = q0*t27*2.0;
t117 = q1*t28*2.0;
t118 = q3*t32*2.0;
t128 = q2*t30*2.0;
t119 = t116+t117+t118-t128;
t120 = q0*t46*2.0;
t121 = q2*t42*2.0;
t122 = q3*t41*2.0;
t129 = q1*t44*2.0;
t123 = t120+t121+t122-t129;
t124 = q1*t52*2.0;
t125 = q2*t51*2.0;
t126 = q3*t49*2.0;
t130 = q0*t54*2.0;
t127 = t124+t125+t126-t130;
t131 = OP_l_8_c_1_r_*t39;
t132 = OP_l_1_c_1_r_*t119;
t141 = OP_l_2_c_1_r_*t123;
t142 = OP_l_3_c_1_r_*t127;
t133 = t131+t132-t141-t142;
t134 = OP_l_8_c_2_r_*t39;
t135 = OP_l_1_c_2_r_*t119;
t147 = OP_l_2_c_2_r_*t123;
t148 = OP_l_3_c_2_r_*t127;
t136 = t134+t135-t147-t148;
t137 = OP_l_8_c_3_r_*t39;
t138 = OP_l_1_c_3_r_*t119;
t153 = OP_l_2_c_3_r_*t123;
t154 = OP_l_3_c_3_r_*t127;
t139 = t137+t138-t153-t154;
t140 = t39*t39;
t143 = q1*t27*2.0;
t144 = q2*t32*2.0;
t145 = q3*t30*2.0;
t204 = q0*t28*2.0;
t146 = t143+t144+t145-t204;
t149 = q0*t44*2.0;
t150 = q1*t46*2.0;
t151 = q2*t41*2.0;
t205 = q3*t42*2.0;
t152 = t149+t150+t151-t205;
t155 = q0*t52*2.0;
t156 = q1*t54*2.0;
t157 = q3*t51*2.0;
t206 = q2*t49*2.0;
t158 = t155+t156+t157-t206;
t159 = t69*t79;
t160 = t71*t75;
t161 = t159+t160;
t162 = t69*t80;
t222 = t71*t78;
t163 = t162-t222;
t164 = t78*t79;
t165 = t75*t80;
t166 = t164+t165;
t167 = OP_l_8_c_11_r_*t39;
t168 = OP_l_1_c_11_r_*t119;
t193 = OP_l_2_c_11_r_*t123;
t194 = OP_l_3_c_11_r_*t127;
t169 = t167+t168-t193-t194;
t170 = t71*t85;
t226 = t79*t88;
t171 = t170-t226;
t172 = t80*t85;
t173 = t79*t93;
t174 = t172+t173;
t175 = OP_l_8_c_10_r_*t39;
t176 = OP_l_1_c_10_r_*t119;
t191 = OP_l_2_c_10_r_*t123;
t192 = OP_l_3_c_10_r_*t127;
t177 = t175+t176-t191-t192;
t178 = OP_l_8_c_12_r_*t39;
t179 = OP_l_1_c_12_r_*t119;
t184 = OP_l_2_c_12_r_*t123;
t185 = OP_l_3_c_12_r_*t127;
t180 = t178+t179-t184-t185;
t181 = t71*t93;
t182 = t80*t88;
t183 = t181+t182;
t186 = t71*t98;
t187 = t80*t99;
t188 = t186+t187;
t189 = t79*t98;
t233 = t80*t102;
t190 = t189-t233;
t195 = t71*t102;
t196 = t79*t99;
t197 = t195+t196;
t198 = OP_l_8_c_7_r_*t39;
t199 = OP_l_1_c_7_r_*t119;
t200 = OP_l_8_c_8_r_*t39;
t201 = OP_l_1_c_8_r_*t119;
t202 = OP_l_8_c_9_r_*t39;
t203 = OP_l_1_c_9_r_*t119;
t207 = OP_l_9_c_1_r_*t39;
t208 = OP_l_2_c_1_r_*t152;
t216 = OP_l_1_c_1_r_*t146;
t217 = OP_l_3_c_1_r_*t158;
t209 = t207+t208-t216-t217;
t210 = OP_l_9_c_2_r_*t39;
t211 = OP_l_2_c_2_r_*t152;
t218 = OP_l_1_c_2_r_*t146;
t219 = OP_l_3_c_2_r_*t158;
t212 = t210+t211-t218-t219;
t213 = OP_l_9_c_3_r_*t39;
t214 = OP_l_2_c_3_r_*t152;
t220 = OP_l_1_c_3_r_*t146;
t221 = OP_l_3_c_3_r_*t158;
t215 = t213+t214-t220-t221;
t223 = OP_l_1_c_11_r_*t146;
t224 = OP_l_3_c_11_r_*t158;
t236 = OP_l_9_c_11_r_*t39;
t237 = OP_l_2_c_11_r_*t152;
t225 = t223+t224-t236-t237;
t227 = OP_l_1_c_10_r_*t146;
t228 = OP_l_3_c_10_r_*t158;
t234 = OP_l_9_c_10_r_*t39;
t235 = OP_l_2_c_10_r_*t152;
t229 = t227+t228-t234-t235;
t230 = OP_l_1_c_12_r_*t146;
t231 = OP_l_3_c_12_r_*t158;
t244 = OP_l_9_c_12_r_*t39;
t245 = OP_l_2_c_12_r_*t152;
t232 = t230+t231-t244-t245;
t238 = OP_l_9_c_7_r_*t39;
t239 = OP_l_2_c_7_r_*t152;
t240 = OP_l_9_c_8_r_*t39;
t241 = OP_l_2_c_8_r_*t152;
t242 = OP_l_9_c_9_r_*t39;
t243 = OP_l_2_c_9_r_*t152;
t246 = OP_l_2_c_1_r_*t163;
t247 = OP_l_11_c_1_r_*t75;
t248 = OP_l_1_c_1_r_*t166;
t258 = OP_l_10_c_1_r_*t69;
t259 = OP_l_3_c_1_r_*t161;
t260 = OP_l_12_c_1_r_*t78;
t249 = OP_l_4_c_1_r_+t246+t247+t248-t258-t259-t260;
t250 = OP_l_2_c_2_r_*t163;
t251 = OP_l_11_c_2_r_*t75;
t252 = OP_l_1_c_2_r_*t166;
t261 = OP_l_10_c_2_r_*t69;
t262 = OP_l_3_c_2_r_*t161;
t263 = OP_l_12_c_2_r_*t78;
t253 = OP_l_4_c_2_r_+t250+t251+t252-t261-t262-t263;
t254 = OP_l_2_c_3_r_*t163;
t255 = OP_l_11_c_3_r_*t75;
t256 = OP_l_1_c_3_r_*t166;
t264 = OP_l_10_c_3_r_*t69;
t265 = OP_l_3_c_3_r_*t161;
t266 = OP_l_12_c_3_r_*t78;
t257 = OP_l_4_c_3_r_+t254+t255+t256-t264-t265-t266;
t267 = OP_l_2_c_10_r_*t163;
t268 = OP_l_11_c_10_r_*t75;
t269 = OP_l_1_c_10_r_*t166;
t279 = OP_l_10_c_10_r_*t69;
t280 = OP_l_3_c_10_r_*t161;
t281 = OP_l_12_c_10_r_*t78;
t270 = OP_l_4_c_10_r_+t267+t268+t269-t279-t280-t281;
t271 = OP_l_2_c_12_r_*t163;
t272 = OP_l_11_c_12_r_*t75;
t273 = OP_l_1_c_12_r_*t166;
t285 = OP_l_10_c_12_r_*t69;
t286 = OP_l_3_c_12_r_*t161;
t287 = OP_l_12_c_12_r_*t78;
t274 = OP_l_4_c_12_r_+t271+t272+t273-t285-t286-t287;
t275 = OP_l_2_c_11_r_*t163;
t276 = OP_l_11_c_11_r_*t75;
t277 = OP_l_1_c_11_r_*t166;
t282 = OP_l_10_c_11_r_*t69;
t283 = OP_l_3_c_11_r_*t161;
t284 = OP_l_12_c_11_r_*t78;
t278 = OP_l_4_c_11_r_+t275+t276+t277-t282-t283-t284;
t288 = OP_l_2_c_7_r_*t163;
t289 = OP_l_11_c_7_r_*t75;
t290 = OP_l_1_c_7_r_*t166;
t291 = OP_l_4_c_7_r_+t288+t289+t290-OP_l_10_c_7_r_*t69-OP_l_3_c_7_r_*t161-OP_l_12_c_7_r_*t78;
t292 = OP_l_2_c_8_r_*t163;
t293 = OP_l_11_c_8_r_*t75;
t294 = OP_l_1_c_8_r_*t166;
t295 = OP_l_4_c_8_r_+t292+t293+t294-OP_l_10_c_8_r_*t69-OP_l_3_c_8_r_*t161-OP_l_12_c_8_r_*t78;
t296 = OP_l_2_c_9_r_*t163;
t297 = OP_l_11_c_9_r_*t75;
t298 = OP_l_1_c_9_r_*t166;
t299 = OP_l_4_c_9_r_+t296+t297+t298-OP_l_10_c_9_r_*t69-OP_l_3_c_9_r_*t161-OP_l_12_c_9_r_*t78;
t300 = OP_l_3_c_1_r_*t171;
t301 = OP_l_12_c_1_r_*t93;
t302 = OP_l_2_c_1_r_*t183;
t312 = OP_l_11_c_1_r_*t85;
t313 = OP_l_1_c_1_r_*t174;
t314 = OP_l_10_c_1_r_*t88;
t303 = OP_l_5_c_1_r_+t300+t301+t302-t312-t313-t314;
t304 = OP_l_3_c_2_r_*t171;
t305 = OP_l_12_c_2_r_*t93;
t306 = OP_l_2_c_2_r_*t183;
t315 = OP_l_11_c_2_r_*t85;
t316 = OP_l_1_c_2_r_*t174;
t317 = OP_l_10_c_2_r_*t88;
t307 = OP_l_5_c_2_r_+t304+t305+t306-t315-t316-t317;
t308 = OP_l_3_c_3_r_*t171;
t309 = OP_l_12_c_3_r_*t93;
t310 = OP_l_2_c_3_r_*t183;
t318 = OP_l_11_c_3_r_*t85;
t319 = OP_l_1_c_3_r_*t174;
t320 = OP_l_10_c_3_r_*t88;
t311 = OP_l_5_c_3_r_+t308+t309+t310-t318-t319-t320;
t321 = dvxNoise*t69*t88;
t322 = OP_l_3_c_10_r_*t171;
t323 = OP_l_12_c_10_r_*t93;
t324 = OP_l_2_c_10_r_*t183;
t334 = OP_l_11_c_10_r_*t85;
t335 = OP_l_1_c_10_r_*t174;
t336 = OP_l_10_c_10_r_*t88;
t325 = OP_l_5_c_10_r_+t322+t323+t324-t334-t335-t336;
t326 = OP_l_3_c_12_r_*t171;
t327 = OP_l_12_c_12_r_*t93;
t328 = OP_l_2_c_12_r_*t183;
t340 = OP_l_11_c_12_r_*t85;
t341 = OP_l_1_c_12_r_*t174;
t342 = OP_l_10_c_12_r_*t88;
t329 = OP_l_5_c_12_r_+t326+t327+t328-t340-t341-t342;
t330 = OP_l_3_c_11_r_*t171;
t331 = OP_l_12_c_11_r_*t93;
t332 = OP_l_2_c_11_r_*t183;
t337 = OP_l_11_c_11_r_*t85;
t338 = OP_l_1_c_11_r_*t174;
t339 = OP_l_10_c_11_r_*t88;
t333 = OP_l_5_c_11_r_+t330+t331+t332-t337-t338-t339;
t343 = OP_l_3_c_7_r_*t171;
t344 = OP_l_12_c_7_r_*t93;
t345 = OP_l_2_c_7_r_*t183;
t346 = OP_l_5_c_7_r_+t343+t344+t345-OP_l_1_c_7_r_*t174-OP_l_10_c_7_r_*t88-OP_l_11_c_7_r_*t85;
t347 = OP_l_3_c_8_r_*t171;
t348 = OP_l_12_c_8_r_*t93;
t349 = OP_l_2_c_8_r_*t183;
t350 = OP_l_5_c_8_r_+t347+t348+t349-OP_l_1_c_8_r_*t174-OP_l_10_c_8_r_*t88-OP_l_11_c_8_r_*t85;
t351 = OP_l_3_c_9_r_*t171;
t352 = OP_l_12_c_9_r_*t93;
t353 = OP_l_2_c_9_r_*t183;
t354 = OP_l_5_c_9_r_+t351+t352+t353-OP_l_1_c_9_r_*t174-OP_l_10_c_9_r_*t88-OP_l_11_c_9_r_*t85;
t355 = OP_l_1_c_1_r_*t190;
t356 = OP_l_10_c_1_r_*t99;
t357 = OP_l_3_c_1_r_*t197;
t367 = OP_l_12_c_1_r_*t98;
t368 = OP_l_2_c_1_r_*t188;
t369 = OP_l_11_c_1_r_*t102;
t358 = OP_l_6_c_1_r_+t355+t356+t357-t367-t368-t369;
t359 = OP_l_1_c_2_r_*t190;
t360 = OP_l_10_c_2_r_*t99;
t361 = OP_l_3_c_2_r_*t197;
t370 = OP_l_12_c_2_r_*t98;
t371 = OP_l_2_c_2_r_*t188;
t372 = OP_l_11_c_2_r_*t102;
t362 = OP_l_6_c_2_r_+t359+t360+t361-t370-t371-t372;
t363 = OP_l_1_c_3_r_*t190;
t364 = OP_l_10_c_3_r_*t99;
t365 = OP_l_3_c_3_r_*t197;
t373 = OP_l_12_c_3_r_*t98;
t374 = OP_l_2_c_3_r_*t188;
t375 = OP_l_11_c_3_r_*t102;
t366 = OP_l_6_c_3_r_+t363+t364+t365-t373-t374-t375;
t376 = dvzNoise*t78*t98;
t377 = OP_l_1_c_10_r_*t190;
t378 = OP_l_10_c_10_r_*t99;
t379 = OP_l_3_c_10_r_*t197;
t390 = OP_l_12_c_10_r_*t98;
t391 = OP_l_2_c_10_r_*t188;
t392 = OP_l_11_c_10_r_*t102;
t380 = OP_l_6_c_10_r_+t377+t378+t379-t390-t391-t392;
t381 = OP_l_1_c_12_r_*t190;
t382 = OP_l_10_c_12_r_*t99;
t383 = OP_l_3_c_12_r_*t197;
t396 = OP_l_12_c_12_r_*t98;
t397 = OP_l_2_c_12_r_*t188;
t398 = OP_l_11_c_12_r_*t102;
t384 = OP_l_6_c_12_r_+t381+t382+t383-t396-t397-t398;
t385 = OP_l_1_c_11_r_*t190;
t386 = OP_l_10_c_11_r_*t99;
t387 = OP_l_3_c_11_r_*t197;
t393 = OP_l_12_c_11_r_*t98;
t394 = OP_l_2_c_11_r_*t188;
t395 = OP_l_11_c_11_r_*t102;
t388 = OP_l_6_c_11_r_+t385+t386+t387-t393-t394-t395;
t389 = dvyNoise*t85*t102;
t399 = OP_l_1_c_7_r_*t190;
t400 = OP_l_10_c_7_r_*t99;
t401 = OP_l_3_c_7_r_*t197;
t402 = OP_l_6_c_7_r_+t399+t400+t401-OP_l_2_c_7_r_*t188-OP_l_11_c_7_r_*t102-OP_l_12_c_7_r_*t98;
t403 = OP_l_1_c_8_r_*t190;
t404 = OP_l_10_c_8_r_*t99;
t405 = OP_l_3_c_8_r_*t197;
t406 = OP_l_6_c_8_r_+t403+t404+t405-OP_l_2_c_8_r_*t188-OP_l_11_c_8_r_*t102-OP_l_12_c_8_r_*t98;
t407 = OP_l_1_c_9_r_*t190;
t408 = OP_l_10_c_9_r_*t99;
t409 = OP_l_3_c_9_r_*t197;
t410 = OP_l_6_c_9_r_+t407+t408+t409-OP_l_2_c_9_r_*t188-OP_l_11_c_9_r_*t102-OP_l_12_c_9_r_*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_l_1_c_4_r_*t34+OP_l_2_c_4_r_*t48-OP_l_3_c_4_r_*t56-OP_l_7_c_4_r_*t39+t75*t84-t69*t91-t78*t97-t70*t161+t166*(t59+t60-t66-t67)+t163*(t62+t63-t72-t73);
A0[0][4] = OP_l_1_c_5_r_*t34+OP_l_2_c_5_r_*t48-OP_l_3_c_5_r_*t56-OP_l_7_c_5_r_*t39-t84*t85-t88*t91+t93*t97-t61*t174+t70*t171+t183*(t62+t63-t72-t73);
A0[0][5] = OP_l_1_c_6_r_*t34+OP_l_2_c_6_r_*t48-OP_l_3_c_6_r_*t56-OP_l_7_c_6_r_*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_l_2_c_7_r_*t123-OP_l_3_c_7_r_*t127);
A0[1][1] = dayNoise*t140+t119*t133-t123*t136-t127*t139+t39*(t200+t201-OP_l_2_c_8_r_*t123-OP_l_3_c_8_r_*t127);
A0[1][2] = -t133*t146+t136*t152-t139*t158+t39*(t202+t203-OP_l_2_c_9_r_*t123-OP_l_3_c_9_r_*t127);
A0[1][3] = -OP_l_8_c_4_r_*t39-OP_l_1_c_4_r_*t119+OP_l_2_c_4_r_*t123+OP_l_3_c_4_r_*t127-t75*t169+t69*t177+t78*t180-t133*t166-t136*t163+t139*t161;
A0[1][4] = -OP_l_8_c_5_r_*t39-OP_l_1_c_5_r_*t119+OP_l_2_c_5_r_*t123+OP_l_3_c_5_r_*t127+t85*t169+t88*t177-t93*t180+t133*t174-t139*t171-t136*t183;
A0[1][5] = -OP_l_8_c_6_r_*t39-OP_l_1_c_6_r_*t119+OP_l_2_c_6_r_*t123+OP_l_3_c_6_r_*t127+t102*t169-t99*t177+t98*t180-t133*t190+t136*t188-t139*t197;
A0[1][6] = -t198-t199+OP_l_2_c_7_r_*t123+OP_l_3_c_7_r_*t127;
A0[1][7] = -t200-t201+OP_l_2_c_8_r_*t123+OP_l_3_c_8_r_*t127;
A0[1][8] = -t202-t203+OP_l_2_c_9_r_*t123+OP_l_3_c_9_r_*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_l_1_c_7_r_*t146-OP_l_3_c_7_r_*t158);
A0[2][1] = t119*t209-t123*t212-t127*t215+t39*(t240+t241-OP_l_1_c_8_r_*t146-OP_l_3_c_8_r_*t158);
A0[2][2] = dazNoise*t140-t146*t209+t152*t212-t158*t215+t39*(t242+t243-OP_l_1_c_9_r_*t146-OP_l_3_c_9_r_*t158);
A0[2][3] = -OP_l_9_c_4_r_*t39+OP_l_1_c_4_r_*t146-OP_l_2_c_4_r_*t152+OP_l_3_c_4_r_*t158-t69*t229+t75*t225-t78*t232-t163*t212-t166*t209+t161*t215;
A0[2][4] = -OP_l_9_c_5_r_*t39+OP_l_1_c_5_r_*t146-OP_l_2_c_5_r_*t152+OP_l_3_c_5_r_*t158-t85*t225-t88*t229+t93*t232+t174*t209-t171*t215-t183*t212;
A0[2][5] = -OP_l_9_c_6_r_*t39+OP_l_1_c_6_r_*t146-OP_l_2_c_6_r_*t152+OP_l_3_c_6_r_*t158-t102*t225-t98*t232-t190*t209+t188*t212-t197*t215+t99*(t227+t228-t234-t235);
A0[2][6] = -t238-t239+OP_l_1_c_7_r_*t146+OP_l_3_c_7_r_*t158;
A0[2][7] = -t240-t241+OP_l_1_c_8_r_*t146+OP_l_3_c_8_r_*t158;
A0[2][8] = -t242-t243+OP_l_1_c_9_r_*t146+OP_l_3_c_9_r_*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_l_4_c_4_r_-OP_l_10_c_4_r_*t69+OP_l_1_c_4_r_*t166+OP_l_2_c_4_r_*t163+OP_l_11_c_4_r_*t75-OP_l_3_c_4_r_*t161-OP_l_12_c_4_r_*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_l_4_c_5_r_+t321-OP_l_10_c_5_r_*t69+OP_l_1_c_5_r_*t166+OP_l_2_c_5_r_*t163+OP_l_11_c_5_r_*t75-OP_l_3_c_5_r_*t161-OP_l_12_c_5_r_*t78-t88*t270-t85*t278+t93*t274-t174*t249+t171*t257+t183*t253-dvyNoise*t75*t85-dvzNoise*t78*t93;
A0[3][5] = OP_l_4_c_6_r_+t376-OP_l_10_c_6_r_*t69+OP_l_1_c_6_r_*t166+OP_l_2_c_6_r_*t163+OP_l_11_c_6_r_*t75-OP_l_3_c_6_r_*t161-OP_l_12_c_6_r_*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_l_5_c_4_r_+t321-OP_l_1_c_4_r_*t174-OP_l_10_c_4_r_*t88-OP_l_11_c_4_r_*t85+OP_l_3_c_4_r_*t171+OP_l_2_c_4_r_*t183+OP_l_12_c_4_r_*t93-t69*t325-t78*t329+t75*t333+t166*t303+t163*t307-t161*t311-dvyNoise*t75*t85-dvzNoise*t78*t93;
A0[4][4] = OP_l_5_c_5_r_-OP_l_1_c_5_r_*t174-OP_l_10_c_5_r_*t88-OP_l_11_c_5_r_*t85+OP_l_3_c_5_r_*t171+OP_l_2_c_5_r_*t183+OP_l_12_c_5_r_*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_l_5_c_6_r_+t389-OP_l_1_c_6_r_*t174-OP_l_10_c_6_r_*t88-OP_l_11_c_6_r_*t85+OP_l_3_c_6_r_*t171+OP_l_2_c_6_r_*t183+OP_l_12_c_6_r_*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_l_6_c_4_r_+t376+OP_l_10_c_4_r_*t99+OP_l_1_c_4_r_*t190-OP_l_2_c_4_r_*t188-OP_l_11_c_4_r_*t102-OP_l_12_c_4_r_*t98+OP_l_3_c_4_r_*t197-t69*t380-t78*t384+t75*t388+t166*t358+t163*t362-t161*t366-dvxNoise*t69*t99-dvyNoise*t75*t102;
A0[5][4] = OP_l_6_c_5_r_+t389+OP_l_10_c_5_r_*t99+OP_l_1_c_5_r_*t190-OP_l_2_c_5_r_*t188-OP_l_11_c_5_r_*t102-OP_l_12_c_5_r_*t98+OP_l_3_c_5_r_*t197-t88*t380-t85*t388+t93*t384-t174*t358+t171*t366+t183*t362-dvxNoise*t88*t99-dvzNoise*t93*t98;
A0[5][5] = OP_l_6_c_6_r_+OP_l_10_c_6_r_*t99+OP_l_1_c_6_r_*t190-OP_l_2_c_6_r_*t188-OP_l_11_c_6_r_*t102-OP_l_12_c_6_r_*t98+OP_l_3_c_6_r_*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_l_7_c_1_r_*t34+OP_l_7_c_2_r_*t48-OP_l_7_c_3_r_*t56;
A0[6][1] = -t412-OP_l_7_c_1_r_*t119+OP_l_7_c_2_r_*t123+OP_l_7_c_3_r_*t127;
A0[6][2] = -t413+OP_l_7_c_1_r_*t146-OP_l_7_c_2_r_*t152+OP_l_7_c_3_r_*t158;
A0[6][3] = OP_l_7_c_4_r_-OP_l_7_c_3_r_*t161+OP_l_7_c_2_r_*t163+OP_l_7_c_1_r_*t166-OP_l_7_c_10_r_*t69+OP_l_7_c_11_r_*t75-OP_l_7_c_12_r_*t78;
A0[6][4] = OP_l_7_c_5_r_+OP_l_7_c_3_r_*t171-OP_l_7_c_1_r_*t174+OP_l_7_c_2_r_*t183-OP_l_7_c_11_r_*t85-OP_l_7_c_10_r_*t88+OP_l_7_c_12_r_*t93;
A0[6][5] = OP_l_7_c_6_r_-OP_l_7_c_2_r_*t188+OP_l_7_c_1_r_*t190+OP_l_7_c_3_r_*t197+OP_l_7_c_10_r_*t99-OP_l_7_c_12_r_*t98-OP_l_7_c_11_r_*t102;
A0[6][6] = OP_l_7_c_7_r_;
A0[6][7] = OP_l_7_c_8_r_;
A0[6][8] = OP_l_7_c_9_r_;
A0[6][9] = OP_l_7_c_10_r_;
A0[6][10] = OP_l_7_c_11_r_;
A0[6][11] = OP_l_7_c_12_r_;
A0[7][0] = -t198+OP_l_8_c_1_r_*t34+OP_l_8_c_2_r_*t48-OP_l_8_c_3_r_*t56;
A0[7][1] = -t200-OP_l_8_c_1_r_*t119+OP_l_8_c_2_r_*t123+OP_l_8_c_3_r_*t127;
A0[7][2] = -t202+OP_l_8_c_1_r_*t146-OP_l_8_c_2_r_*t152+OP_l_8_c_3_r_*t158;
A0[7][3] = OP_l_8_c_4_r_-OP_l_8_c_3_r_*t161+OP_l_8_c_2_r_*t163+OP_l_8_c_1_r_*t166-OP_l_8_c_10_r_*t69+OP_l_8_c_11_r_*t75-OP_l_8_c_12_r_*t78;
A0[7][4] = OP_l_8_c_5_r_+OP_l_8_c_3_r_*t171-OP_l_8_c_1_r_*t174+OP_l_8_c_2_r_*t183-OP_l_8_c_11_r_*t85-OP_l_8_c_10_r_*t88+OP_l_8_c_12_r_*t93;
A0[7][5] = OP_l_8_c_6_r_-OP_l_8_c_2_r_*t188+OP_l_8_c_1_r_*t190+OP_l_8_c_3_r_*t197+OP_l_8_c_10_r_*t99-OP_l_8_c_12_r_*t98-OP_l_8_c_11_r_*t102;
A0[7][6] = OP_l_8_c_7_r_;
A0[7][7] = OP_l_8_c_8_r_;
A0[7][8] = OP_l_8_c_9_r_;
A0[7][9] = OP_l_8_c_10_r_;
A0[7][10] = OP_l_8_c_11_r_;
A0[7][11] = OP_l_8_c_12_r_;
A0[8][0] = -t238+OP_l_9_c_1_r_*t34+OP_l_9_c_2_r_*t48-OP_l_9_c_3_r_*t56;
A0[8][1] = -t240-OP_l_9_c_1_r_*t119+OP_l_9_c_2_r_*t123+OP_l_9_c_3_r_*t127;
A0[8][2] = -t242+OP_l_9_c_1_r_*t146-OP_l_9_c_2_r_*t152+OP_l_9_c_3_r_*t158;
A0[8][3] = OP_l_9_c_4_r_-OP_l_9_c_3_r_*t161+OP_l_9_c_2_r_*t163+OP_l_9_c_1_r_*t166-OP_l_9_c_10_r_*t69+OP_l_9_c_11_r_*t75-OP_l_9_c_12_r_*t78;
A0[8][4] = OP_l_9_c_5_r_+OP_l_9_c_3_r_*t171-OP_l_9_c_1_r_*t174+OP_l_9_c_2_r_*t183-OP_l_9_c_11_r_*t85-OP_l_9_c_10_r_*t88+OP_l_9_c_12_r_*t93;
A0[8][5] = OP_l_9_c_6_r_-OP_l_9_c_2_r_*t188+OP_l_9_c_1_r_*t190+OP_l_9_c_3_r_*t197+OP_l_9_c_10_r_*t99-OP_l_9_c_12_r_*t98-OP_l_9_c_11_r_*t102;
A0[8][6] = OP_l_9_c_7_r_;
A0[8][7] = OP_l_9_c_8_r_;
A0[8][8] = OP_l_9_c_9_r_;
A0[8][9] = OP_l_9_c_10_r_;
A0[8][10] = OP_l_9_c_11_r_;
A0[8][11] = OP_l_9_c_12_r_;
A0[9][0] = OP_l_10_c_1_r_*t34-OP_l_10_c_7_r_*t39+OP_l_10_c_2_r_*t48-OP_l_10_c_3_r_*t56;
A0[9][1] = -OP_l_10_c_8_r_*t39-OP_l_10_c_1_r_*t119+OP_l_10_c_2_r_*t123+OP_l_10_c_3_r_*t127;
A0[9][2] = -OP_l_10_c_9_r_*t39+OP_l_10_c_1_r_*t146-OP_l_10_c_2_r_*t152+OP_l_10_c_3_r_*t158;
A0[9][3] = OP_l_10_c_4_r_-t279-OP_l_10_c_3_r_*t161+OP_l_10_c_2_r_*t163+OP_l_10_c_1_r_*t166+OP_l_10_c_11_r_*t75-OP_l_10_c_12_r_*t78;
A0[9][4] = OP_l_10_c_5_r_-t336+OP_l_10_c_3_r_*t171-OP_l_10_c_1_r_*t174+OP_l_10_c_2_r_*t183-OP_l_10_c_11_r_*t85+OP_l_10_c_12_r_*t93;
A0[9][5] = OP_l_10_c_6_r_+t378-OP_l_10_c_2_r_*t188+OP_l_10_c_1_r_*t190+OP_l_10_c_3_r_*t197-OP_l_10_c_12_r_*t98-OP_l_10_c_11_r_*t102;
A0[9][6] = OP_l_10_c_7_r_;
A0[9][7] = OP_l_10_c_8_r_;
A0[9][8] = OP_l_10_c_9_r_;
A0[9][9] = OP_l_10_c_10_r_;
A0[9][10] = OP_l_10_c_11_r_;
A0[9][11] = OP_l_10_c_12_r_;
A0[10][0] = OP_l_11_c_1_r_*t34-OP_l_11_c_7_r_*t39+OP_l_11_c_2_r_*t48-OP_l_11_c_3_r_*t56;
A0[10][1] = -OP_l_11_c_8_r_*t39-OP_l_11_c_1_r_*t119+OP_l_11_c_2_r_*t123+OP_l_11_c_3_r_*t127;
A0[10][2] = -OP_l_11_c_9_r_*t39+OP_l_11_c_1_r_*t146-OP_l_11_c_2_r_*t152+OP_l_11_c_3_r_*t158;
A0[10][3] = OP_l_11_c_4_r_+t276-OP_l_11_c_3_r_*t161+OP_l_11_c_2_r_*t163+OP_l_11_c_1_r_*t166-OP_l_11_c_10_r_*t69-OP_l_11_c_12_r_*t78;
A0[10][4] = OP_l_11_c_5_r_-t337+OP_l_11_c_3_r_*t171-OP_l_11_c_1_r_*t174+OP_l_11_c_2_r_*t183-OP_l_11_c_10_r_*t88+OP_l_11_c_12_r_*t93;
A0[10][5] = OP_l_11_c_6_r_-t395-OP_l_11_c_2_r_*t188+OP_l_11_c_1_r_*t190+OP_l_11_c_3_r_*t197+OP_l_11_c_10_r_*t99-OP_l_11_c_12_r_*t98;
A0[10][6] = OP_l_11_c_7_r_;
A0[10][7] = OP_l_11_c_8_r_;
A0[10][8] = OP_l_11_c_9_r_;
A0[10][9] = OP_l_11_c_10_r_;
A0[10][10] = OP_l_11_c_11_r_;
A0[10][11] = OP_l_11_c_12_r_;
A0[11][0] = OP_l_12_c_1_r_*t34-OP_l_12_c_7_r_*t39+OP_l_12_c_2_r_*t48-OP_l_12_c_3_r_*t56;
A0[11][1] = -OP_l_12_c_8_r_*t39-OP_l_12_c_1_r_*t119+OP_l_12_c_2_r_*t123+OP_l_12_c_3_r_*t127;
A0[11][2] = -OP_l_12_c_9_r_*t39+OP_l_12_c_1_r_*t146-OP_l_12_c_2_r_*t152+OP_l_12_c_3_r_*t158;
A0[11][3] = OP_l_12_c_4_r_-t287-OP_l_12_c_3_r_*t161+OP_l_12_c_2_r_*t163+OP_l_12_c_1_r_*t166-OP_l_12_c_10_r_*t69+OP_l_12_c_11_r_*t75;
A0[11][4] = OP_l_12_c_5_r_+t327+OP_l_12_c_3_r_*t171-OP_l_12_c_1_r_*t174+OP_l_12_c_2_r_*t183-OP_l_12_c_11_r_*t85-OP_l_12_c_10_r_*t88;
A0[11][5] = OP_l_12_c_6_r_-t396-OP_l_12_c_2_r_*t188+OP_l_12_c_1_r_*t190+OP_l_12_c_3_r_*t197+OP_l_12_c_10_r_*t99-OP_l_12_c_11_r_*t102;
A0[11][6] = OP_l_12_c_7_r_;
A0[11][7] = OP_l_12_c_8_r_;
A0[11][8] = OP_l_12_c_9_r_;
A0[11][9] = OP_l_12_c_10_r_;
A0[11][10] = OP_l_12_c_11_r_;
A0[11][11] = OP_l_12_c_12_r_;