bortzh2.m

地理坐标系内的航迹仿真GPS 地理坐标系内的航迹仿真 地理坐标系内的航迹仿真

function dOmegadRhoDot = BortzH2(rho);
%
% Computes the measurement sensitivity matrix for measured body rate vector
% omega to Bortz rotation rate vector rhodot, as a function of rho.
%
t1 = rho(1) ^ 2;
t2 = t1 ^ 2;
t3 = rho(2) ^ 2;
t4 = rho(3) ^ 2;
t5 = t1 + t3 + t4;
t6 = sqrt(t5);
t7 = cos(t6);
t8 = t7 ^ 2;
t12 = t1 * t7;
t14 = 0.2e1 * t12 * t4;
t16 = 0.2e1 * t12 * t3;
t17 = t8 * t1;
t18 = t17 * t3;
t19 = t17 * t4;
t20 = t1 * t3;
t21 = t1 * t4;
t22 = sin(t6);
t23 = t22 ^ 2;
t25 = t1 * t23;
t26 = t25 * t3;
t27 = t25 * t4;
t28 = t6 * t22;
t30 = 0.2e1 * t28 * t3;
t32 = 0.2e1 * t28 * t4;
t33 = t7 * t3;
t35 = 0.2e1 * t33 * t28;
t36 = t7 * t4;
t38 = 0.2e1 * t28 * t36;
t39 = t2 + t8 * t2 - 0.2e1 * t2 * t7 - t14 - t16 + t18 + t19 + t20 + t21 + t2 * t23 + t26 + t27 + t30 + t32 - t35 - t38;
t40 = 0.1e1 / t5;
t44 = t8 * t3;
t45 = t8 * t4;
t47 = t23 * t3;
t49 = t1 + t17 - 0.2e1 * t12 + t25 - 0.2e1 * t36 + t44 + t45 + t3 - 0.2e1 * t33 + t47 + t23 * t4 + t4;
t50 = 0.1e1 / t49;
t52 = t1 * rho(1);
t53 = t52 * rho(2);
t55 = 0.2e1 * t53 * t7;
t56 = t52 * t8;
t57 = t56 * rho(2);
t58 = rho(3) * t1;
t59 = 0.2e1 * t58;
t60 = rho(3) * t7;
t62 = 0.4e1 * t60 * t1;
t64 = 0.2e1 * t17 * rho(3);
t65 = t52 * t23;
t66 = t65 * rho(2);
t67 = rho(1) * t23;
t68 = t3 * rho(2);
t69 = t67 * t68;
t70 = rho(2) * t4;
t71 = t67 * t70;
t72 = rho(2) * rho(1);
t74 = 0.2e1 * t72 * t36;
t75 = rho(1) * t7;
t77 = 0.2e1 * t75 * t68;
t78 = t53 - t55 + t57 + t59 - t62 + t64 + t66 + t69 + t71 - t74 - t77;
t79 = t72 * t45;
t80 = rho(1) * t68;
t82 = rho(2) * t6 * t22;
t84 = 0.2e1 * t75 * t82;
t85 = t80 * t8;
t87 = 0.2e1 * t72 * t28;
t88 = t72 * t4;
t89 = t3 * rho(3);
t90 = 0.2e1 * t89;
t91 = t4 * rho(3);
t92 = t91 * t8;
t93 = 0.2e1 * t92;
t95 = 0.4e1 * t7 * t91;
t97 = 0.4e1 * t60 * t3;
t98 = 0.2e1 * t91;
t100 = 0.2e1 * t44 * rho(3);
t101 = t79 + t80 + t84 + t85 - t87 + t88 + t90 + t93 - t95 - t97 + t98 + t100;
t105 = t52 * rho(3);
t107 = 0.2e1 * t105 * t7;
t108 = t56 * rho(3);
t109 = rho(2) * t7;
t111 = 0.4e1 * t109 * t1;
t113 = 0.2e1 * t17 * rho(2);
t115 = 0.2e1 * rho(2) * t1;
t116 = rho(1) * t3;
t117 = t8 * rho(3);
t118 = t116 * t117;
t121 = 0.2e1 * rho(3) * rho(1) * t28;
t123 = 0.2e1 * t116 * t60;
t125 = 0.2e1 * t75 * t91;
t126 = t65 * rho(3);
t127 = -t107 + t108 + t105 + t111 - t113 - t115 + t118 - t121 - t123 - t125 + t126;
t128 = t67 * t89;
t129 = t67 * t91;
t134 = 0.2e1 * rho(1) * t6 * t22 * t7 * rho(3);
t135 = t116 * rho(3);
t136 = rho(1) * t91;
t137 = t136 * t8;
t140 = 0.2e1 * rho(2) * t8 * t4;
t142 = 0.4e1 * t109 * t4;
t144 = 0.2e1 * t68 * t8;
t145 = 0.2e1 * t68;
t147 = 0.4e1 * t7 * t68;
t148 = 0.2e1 * t70;
t149 = t128 + t129 + t134 + t135 + t136 + t137 - t140 + t142 - t144 - t145 + t147 - t148;
t153 = t53 - t55 + t57 - t59 + t62 - t64 + t66 + t69 + t71 - t74 - t77;
t154 = t79 + t80 + t84 + t85 - t87 + t88 - t90 - t98 + t97 + t95 - t100 - t93;
t159 = 0.2e1 * t12 * t28;
t161 = 0.2e1 * t28 * t1;
t162 = t3 ^ 2;
t166 = t47 * t4;
t168 = 0.2e1 * t33 * t4;
t169 = t44 * t4;
t170 = t3 * t4;
t172 = -t16 - t159 + t20 + t161 + t18 + t32 - 0.2e1 * t162 * t7 + t26 + t162 * t23 + t166 - t168 + t169 + t162 - t38 + t170 + t8 * t162;
t176 = 0.4e1 * t7 * t52;
t177 = 0.2e1 * t56;
t178 = 0.2e1 * t52;
t180 = 0.2e1 * t58 * t109;
t181 = t58 * rho(2);
t182 = rho(3) * rho(2);
t183 = t17 * t182;
t185 = 0.2e1 * rho(1) * t4;
t187 = 0.4e1 * t75 * t4;
t188 = 0.2e1 * t116;
t189 = rho(1) * t8;
t191 = 0.2e1 * t189 * t4;
t193 = 0.2e1 * t189 * t3;
t194 = -t176 + t177 + t178 - t180 + t181 + t183 + t185 - t187 + t188 + t191 + t193;
t196 = 0.4e1 * t75 * t3;
t198 = 0.2e1 * t60 * t68;
t199 = t91 * rho(2);
t201 = 0.2e1 * t199 * t7;
t202 = t117 * t68;
t203 = rho(3) * t68;
t205 = 0.2e1 * t60 * t82;
t206 = t92 * rho(2);
t207 = rho(2) * t23;
t208 = t207 * t58;
t210 = t68 * t23 * rho(3);
t211 = t207 * t91;
t213 = 0.2e1 * t182 * t28;
t214 = -t196 - t198 - t201 + t202 + t199 + t203 + t205 + t206 + t208 + t210 + t211 - t213;
t218 = -t107 + t108 + t105 - t111 + t113 + t115 + t118 - t121 - t123 - t125 + t126;
t219 = t128 + t129 + t134 + t135 + t136 + t137 - t147 - t142 + t144 + t140 + t145 + t148;
t223 = -t176 + t177 + t178 + t180 - t181 - t183 + t185 - t187 + t188 + t191 + t193;
t224 = -t196 + t198 + t201 - t202 - t199 - t203 - t205 - t206 - t208 - t210 - t211 + t213;
t228 = t4 ^ 2;
t233 = t161 - t14 + t21 - t159 + t19 - t35 + t27 + t166 + t228 * t23 + t169 + t170 - 0.2e1 * t228 * t7 - t168 + t8 * t228 + t228 + t30;
dOmegadRhoDot(1,1) = t39 * t40 * t50;
dOmegadRhoDot(1,2) = (t78 + t101) * t40 * t50;
dOmegadRhoDot(1,3) = (t127 + t149) * t40 * t50;
dOmegadRhoDot(2,1) = (t153 + t154) * t40 * t50;
dOmegadRhoDot(2,2) = t172 * t40 * t50;
dOmegadRhoDot(2,3) = (t194 + t214) * t40 * t50;
dOmegadRhoDot(3,1) = (t218 + t219) * t40 * t50;
dOmegadRhoDot(3,2) = -(t223 + t224) * t40 * t50;
dOmegadRhoDot(3,3) = t233 * t40 * t50;