ardupilot/libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcP.m

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

421 lines
25 KiB
Mathematica
Raw Normal View History

function PP = calcP(OP_l_1_c_1_r_,OP_l_1_c_2_r_,OP_l_1_c_3_r_,OP_l_1_c_4_r_,OP_l_1_c_5_r_,OP_l_1_c_6_r_,OP_l_1_c_7_r_,OP_l_1_c_8_r_,OP_l_1_c_9_r_,OP_l_2_c_1_r_,OP_l_2_c_2_r_,OP_l_2_c_3_r_,OP_l_2_c_4_r_,OP_l_2_c_5_r_,OP_l_2_c_6_r_,OP_l_2_c_7_r_,OP_l_2_c_8_r_,OP_l_2_c_9_r_,OP_l_3_c_1_r_,OP_l_3_c_2_r_,OP_l_3_c_3_r_,OP_l_3_c_4_r_,OP_l_3_c_5_r_,OP_l_3_c_6_r_,OP_l_3_c_7_r_,OP_l_3_c_8_r_,OP_l_3_c_9_r_,OP_l_4_c_1_r_,OP_l_4_c_2_r_,OP_l_4_c_3_r_,OP_l_4_c_4_r_,OP_l_4_c_5_r_,OP_l_4_c_6_r_,OP_l_4_c_7_r_,OP_l_4_c_8_r_,OP_l_4_c_9_r_,OP_l_5_c_1_r_,OP_l_5_c_2_r_,OP_l_5_c_3_r_,OP_l_5_c_4_r_,OP_l_5_c_5_r_,OP_l_5_c_6_r_,OP_l_5_c_7_r_,OP_l_5_c_8_r_,OP_l_5_c_9_r_,OP_l_6_c_1_r_,OP_l_6_c_2_r_,OP_l_6_c_3_r_,OP_l_6_c_4_r_,OP_l_6_c_5_r_,OP_l_6_c_6_r_,OP_l_6_c_7_r_,OP_l_6_c_8_r_,OP_l_6_c_9_r_,OP_l_7_c_1_r_,OP_l_7_c_2_r_,OP_l_7_c_3_r_,OP_l_7_c_4_r_,OP_l_7_c_5_r_,OP_l_7_c_6_r_,OP_l_7_c_7_r_,OP_l_7_c_8_r_,OP_l_7_c_9_r_,OP_l_8_c_1_r_,OP_l_8_c_2_r_,OP_l_8_c_3_r_,OP_l_8_c_4_r_,OP_l_8_c_5_r_,OP_l_8_c_6_r_,OP_l_8_c_7_r_,OP_l_8_c_8_r_,OP_l_8_c_9_r_,OP_l_9_c_1_r_,OP_l_9_c_2_r_,OP_l_9_c_3_r_,OP_l_9_c_4_r_,OP_l_9_c_5_r_,OP_l_9_c_6_r_,OP_l_9_c_7_r_,OP_l_9_c_8_r_,OP_l_9_c_9_r_,OP_l_10_c_1_r_,OP_l_10_c_2_r_,OP_l_10_c_3_r_,OP_l_10_c_4_r_,OP_l_10_c_5_r_,OP_l_10_c_6_r_,OP_l_10_c_7_r_,OP_l_10_c_8_r_,OP_l_10_c_9_r_,OP_l_1_c_10_r_,OP_l_11_c_1_r_,OP_l_1_c_11_r_,OP_l_11_c_2_r_,OP_l_1_c_12_r_,OP_l_11_c_3_r_,OP_l_11_c_4_r_,OP_l_11_c_5_r_,OP_l_11_c_6_r_,OP_l_11_c_7_r_,OP_l_11_c_8_r_,OP_l_11_c_9_r_,OP_l_12_c_1_r_,OP_l_12_c_2_r_,OP_l_12_c_3_r_,OP_l_12_c_4_r_,OP_l_12_c_5_r_,OP_l_12_c_6_r_,OP_l_12_c_7_r_,OP_l_12_c_8_r_,OP_l_12_c_9_r_,OP_l_2_c_10_r_,OP_l_2_c_11_r_,OP_l_2_c_12_r_,OP_l_3_c_10_r_,OP_l_3_c_11_r_,OP_l_3_c_12_r_,OP_l_4_c_10_r_,OP_l_4_c_11_r_,OP_l_4_c_12_r_,OP_l_5_c_10_r_,OP_l_5_c_11_r_,OP_l_5_c_12_r_,OP_l_6_c_10_r_,OP_l_6_c_11_r_,OP_l_6_c_12_r_,OP_l_7_c_10_r_,OP_l_7_c_11_r_,OP_l_7_c_12_r_,OP_l_8_c_10_r_,OP_l_8_c_11_r_,OP_l_8_c_12_r_,OP_l_9_c_10_r_,OP_l_9_c_11_r_,OP_l_9_c_12_r_,OP_l_10_c_10_r_,OP_l_10_c_11_r_,OP_l_10_c_12_r_,OP_l_11_c_10_r_,OP_l_11_c_11_r_,OP_l_11_c_12_r_,OP_l_12_c_10_r_,OP_l_12_c_11_r_,OP_l_12_c_12_r_,dax,dax_b,day,day_b,daxNoise,daz,daz_b,dayNoise,dazNoise,dvx,dvx_b,dvy,dvy_b,dvxNoise,dvz,dvz_b,dvyNoise,dvzNoise,q0,q1,q2,q3)
%CALCP
% PP = CALCP(OP_L_1_C_1_R_,OP_L_1_C_2_R_,OP_L_1_C_3_R_,OP_L_1_C_4_R_,OP_L_1_C_5_R_,OP_L_1_C_6_R_,OP_L_1_C_7_R_,OP_L_1_C_8_R_,OP_L_1_C_9_R_,OP_L_2_C_1_R_,OP_L_2_C_2_R_,OP_L_2_C_3_R_,OP_L_2_C_4_R_,OP_L_2_C_5_R_,OP_L_2_C_6_R_,OP_L_2_C_7_R_,OP_L_2_C_8_R_,OP_L_2_C_9_R_,OP_L_3_C_1_R_,OP_L_3_C_2_R_,OP_L_3_C_3_R_,OP_L_3_C_4_R_,OP_L_3_C_5_R_,OP_L_3_C_6_R_,OP_L_3_C_7_R_,OP_L_3_C_8_R_,OP_L_3_C_9_R_,OP_L_4_C_1_R_,OP_L_4_C_2_R_,OP_L_4_C_3_R_,OP_L_4_C_4_R_,OP_L_4_C_5_R_,OP_L_4_C_6_R_,OP_L_4_C_7_R_,OP_L_4_C_8_R_,OP_L_4_C_9_R_,OP_L_5_C_1_R_,OP_L_5_C_2_R_,OP_L_5_C_3_R_,OP_L_5_C_4_R_,OP_L_5_C_5_R_,OP_L_5_C_6_R_,OP_L_5_C_7_R_,OP_L_5_C_8_R_,OP_L_5_C_9_R_,OP_L_6_C_1_R_,OP_L_6_C_2_R_,OP_L_6_C_3_R_,OP_L_6_C_4_R_,OP_L_6_C_5_R_,OP_L_6_C_6_R_,OP_L_6_C_7_R_,OP_L_6_C_8_R_,OP_L_6_C_9_R_,OP_L_7_C_1_R_,OP_L_7_C_2_R_,OP_L_7_C_3_R_,OP_L_7_C_4_R_,OP_L_7_C_5_R_,OP_L_7_C_6_R_,OP_L_7_C_7_R_,OP_L_7_C_8_R_,OP_L_7_C_9_R_,OP_L_8_C_1_R_,OP_L_8_C_2_R_,OP_L_8_C_3_R_,OP_L_8_C_4_R_,OP_L_8_C_5_R_,OP_L_8_C_6_R_,OP_L_8_C_7_R_,OP_L_8_C_8_R_,OP_L_8_C_9_R_,OP_L_9_C_1_R_,OP_L_9_C_2_R_,OP_L_9_C_3_R_,OP_L_9_C_4_R_,OP_L_9_C_5_R_,OP_L_9_C_6_R_,OP_L_9_C_7_R_,OP_L_9_C_8_R_,OP_L_9_C_9_R_,OP_L_10_C_1_R_,OP_L_10_C_2_R_,OP_L_10_C_3_R_,OP_L_10_C_4_R_,OP_L_10_C_5_R_,OP_L_10_C_6_R_,OP_L_10_C_7_R_,OP_L_10_C_8_R_,OP_L_10_C_9_R_,OP_L_1_C_10_R_,OP_L_11_C_1_R_,OP_L_1_C_11_R_,OP_L_11_C_2_R_,OP_L_1_C_12_R_,OP_L_11_C_3_R_,OP_L_11_C_4_R_,OP_L_11_C_5_R_,OP_L_11_C_6_R_,OP_L_11_C_7_R_,OP_L_11_C_8_R_,OP_L_11_C_9_R_,OP_L_12_C_1_R_,OP_L_12_C_2_R_,OP_L_12_C_3_R_,OP_L_12_C_4_R_,OP_L_12_C_5_R_,OP_L_12_C_6_R_,OP_L_12_C_7_R_,OP_L_12_C_8_R_,OP_L_12_C_9_R_,OP_L_2_C_10_R_,OP_L_2_C_11_R_,OP_L_2_C_12_R_,OP_L_3_C_10_R_,OP_L_3_C_11_R_,OP_L_3_C_12_R_,OP_L_4_C_10_R_,OP_L_4_C_11_R_,OP_L_4_C_12_R_,OP_L_5_C_10_R_,OP_L_5_C_11_R_,OP_L_5_C_12_R_,OP_L_6_C_10_R_,OP_L_6_C_11_R_,OP_L_6_C_12_R_,OP_L_7_C_10_R_,OP_L_7_C_11_R_,OP_L_7_C_12_R_,OP_L_8_C_10_R_,OP_L_8_C_11_R_,OP_L_8_C_12_R_,OP_L_9_C_10_R_,OP_L_9_C_11_R_,OP_L_9_C_12_R_,OP_L_10_C_10_R_,OP_L_10_C_11_R_,OP_L_10_C_12_R_,OP_L_11_C_10_R_,OP_L_11_C_11_R_,OP_L_11_C_12_R_,OP_L_12_C_10_R_,OP_L_12_C_11_R_,OP_L_12_C_12_R_,DAX,DAX_B,DAY,DAY_B,DAXNOISE,DAZ,DAZ_B,DAYNOISE,DAZNOISE,DVX,DVX_B,DVY,DVY_B,DVXNOISE,DVZ,DVZ_B,DVYNOISE,DVZNOISE,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.1.
% 15-Feb-2015 15:49:36
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.^2;
t36 = q1.^2;
t37 = q2.^2;
t38 = q3.^2;
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.^2;
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;
PP = reshape([daxNoise.*t140+t34.*t61+t48.*t68-t56.*t70-t39.*t107,-t34.*t133-t48.*t136+t56.*t139+t39.*(t198+t199-OP_l_2_c_7_r_.*t123-OP_l_3_c_7_r_.*t127),-t34.*t209-t48.*t212+t56.*t215+t39.*(t238+t239-OP_l_1_c_7_r_.*t146-OP_l_3_c_7_r_.*t158),t34.*t249+t48.*t253-t56.*t257-t39.*t291,t34.*t303+t48.*t307-t56.*t311-t39.*t346,t34.*t358+t48.*t362-t56.*t366-t39.*t402,-t411+OP_l_7_c_1_r_.*t34+OP_l_7_c_2_r_.*t48-OP_l_7_c_3_r_.*t56,-t198+OP_l_8_c_1_r_.*t34+OP_l_8_c_2_r_.*t48-OP_l_8_c_3_r_.*t56,-t238+OP_l_9_c_1_r_.*t34+OP_l_9_c_2_r_.*t48-OP_l_9_c_3_r_.*t56,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,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,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,-t39.*t110-t61.*t119+t68.*t123+t70.*t127,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),t119.*t209-t123.*t212-t127.*t215+t39.*(t240+t241-OP_l_1_c_8_r_.*t146-OP_l_3_c_8_r_.*t158),-t39.*t295-t119.*t249+t123.*t253+t127.*t257,-t39.*t350-t119.*t303+t123.*t307+t127.*t311,-t39.*t406-t119.*t358+t123.*t362+t127.*t366,-t412-OP_l_7_c_1_r_.*t119+OP_l_7_c_2_r_.*t123+OP_l_7_c_3_r_.*t127,-t200-OP_l_8_c_1_r_.*t119+OP_l_8_c_2_r_.*t123+OP_l_8_c_3_r_.*t127,-t240-OP_l_9_c_1_r_.*t119+OP_l_9_c_2_r_.*t123+OP_l_9_c_3_r_.*t127,-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,-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,-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,-t39.*t113-t68.*t152+t70.*t158+t146.*(t59+t60-t66-t67),-t133.*t146+t136.*t152-t139.*t158+t39.*(t202+t203-OP_l_2_c_9_r_.*t123-OP_l_3_c_9_r_.*t127),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),-t39.*t299+t146.*t249-t152.*t253+t158.*t257,-t39.*t354+t146.*t303-t152.*t307+t158.*t311,-t39.*t410+t146.*t358-t152.*t362+t158.*t366,-t413+OP_l_7_c_1_r_.*t146-OP_l_7_c_2_r_.*t152+OP_l_7_c_3_r_.*t158,-t202+OP_l_8_c_1_r_.*t146-OP_l_8_c_2_r_.*t152+OP_l_8_c_3_r_.*t158,-t242+OP_l_9_c_1_r_.*t146-OP_l_9_c_2_r_.*t152+OP_l_9_c_3_r_.*t158,-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,-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,-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,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),-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,-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,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.^2+dvyNoise.*t75.^2+dvzNoise.*t78.^2,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,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,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,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,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,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_