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/C_code.txt

348 lines
11 KiB
Plaintext
Raw Blame History

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[0][0]*t34;
t60 = OP[1][0]*t48;
t66 = OP[6][0]*t39;
t67 = OP[2][0]*t56;
t61 = t59+t60-t66-t67;
t62 = OP[0][1]*t34;
t63 = OP[1][1]*t48;
t64 = OP[0][2]*t34;
t65 = OP[1][2]*t48;
t71 = OP[6][1]*t39;
t72 = OP[2][1]*t56;
t68 = t62+t63-t71-t72;
t79 = OP[6][2]*t39;
t80 = OP[2][2]*t56;
t69 = t64+t65-t79-t80;
t70 = t35+t36-t37-t38;
t73 = q0*q2*2.0;
t74 = q1*q3*2.0;
t75 = t73+t74;
t76 = q0*q3*2.0;
t78 = q1*q2*2.0;
t77 = t76-t78;
t81 = t35-t36+t37-t38;
t82 = q0*q1*2.0;
t86 = q2*q3*2.0;
t83 = t82-t86;
t84 = t76+t78;
t85 = t35-t36-t37+t38;
t87 = t82+t86;
t88 = t73-t74;
t89 = OP[0][6]*t34;
t90 = OP[1][6]*t48;
t265 = OP[6][6]*t39;
t91 = t89+t90-t265-OP[2][6]*t56;
t92 = OP[0][7]*t34;
t93 = OP[1][7]*t48;
t266 = OP[6][7]*t39;
t94 = t92+t93-t266-OP[2][7]*t56;
t95 = OP[0][8]*t34;
t96 = OP[1][8]*t48;
t267 = OP[6][8]*t39;
t97 = t95+t96-t267-OP[2][8]*t56;
t98 = q0*t27*2.0;
t99 = q1*t28*2.0;
t100 = q3*t32*2.0;
t110 = q2*t30*2.0;
t101 = t98+t99+t100-t110;
t102 = q0*t46*2.0;
t103 = q2*t42*2.0;
t104 = q3*t41*2.0;
t111 = q1*t44*2.0;
t105 = t102+t103+t104-t111;
t106 = q1*t52*2.0;
t107 = q2*t51*2.0;
t108 = q3*t49*2.0;
t112 = q0*t54*2.0;
t109 = t106+t107+t108-t112;
t113 = OP[7][0]*t39;
t114 = OP[0][0]*t101;
t123 = OP[1][0]*t105;
t124 = OP[2][0]*t109;
t115 = t113+t114-t123-t124;
t116 = OP[7][1]*t39;
t117 = OP[0][1]*t101;
t129 = OP[1][1]*t105;
t130 = OP[2][1]*t109;
t118 = t116+t117-t129-t130;
t119 = OP[7][2]*t39;
t120 = OP[0][2]*t101;
t135 = OP[1][2]*t105;
t136 = OP[2][2]*t109;
t121 = t119+t120-t135-t136;
t122 = t39*t39;
t125 = q1*t27*2.0;
t126 = q2*t32*2.0;
t127 = q3*t30*2.0;
t170 = q0*t28*2.0;
t128 = t125+t126+t127-t170;
t131 = q0*t44*2.0;
t132 = q1*t46*2.0;
t133 = q2*t41*2.0;
t171 = q3*t42*2.0;
t134 = t131+t132+t133-t171;
t137 = q0*t52*2.0;
t138 = q1*t54*2.0;
t139 = q3*t51*2.0;
t172 = q2*t49*2.0;
t140 = t137+t138+t139-t172;
t141 = dvy*t70;
t142 = dvx*t77;
t143 = t141+t142;
t144 = dvx*t75;
t145 = dvy*t75;
t146 = dvz*t77;
t147 = t145+t146;
t148 = dvx*t81;
t188 = dvy*t84;
t149 = t148-t188;
t150 = dvz*t81;
t151 = dvy*t83;
t152 = t150+t151;
t153 = dvx*t83;
t154 = dvz*t84;
t155 = t153+t154;
t156 = dvx*t85;
t157 = dvz*t88;
t158 = t156+t157;
t159 = dvy*t85;
t189 = dvz*t87;
t160 = t159-t189;
t161 = dvx*t87;
t162 = dvy*t88;
t163 = t161+t162;
t164 = OP[7][6]*t39;
t165 = OP[0][6]*t101;
t166 = OP[7][7]*t39;
t167 = OP[0][7]*t101;
t168 = OP[7][8]*t39;
t169 = OP[0][8]*t101;
t173 = OP[8][0]*t39;
t174 = OP[1][0]*t134;
t182 = OP[0][0]*t128;
t183 = OP[2][0]*t140;
t175 = t173+t174-t182-t183;
t176 = OP[8][1]*t39;
t177 = OP[1][1]*t134;
t184 = OP[0][1]*t128;
t185 = OP[2][1]*t140;
t178 = t176+t177-t184-t185;
t179 = OP[8][2]*t39;
t180 = OP[1][2]*t134;
t186 = OP[0][2]*t128;
t187 = OP[2][2]*t140;
t181 = t179+t180-t186-t187;
t190 = OP[8][6]*t39;
t191 = OP[1][6]*t134;
t192 = OP[8][7]*t39;
t193 = OP[1][7]*t134;
t194 = OP[8][8]*t39;
t195 = OP[1][8]*t134;
t197 = dvz*t70;
t196 = t144-t197;
t198 = OP[0][0]*t147;
t204 = OP[2][0]*t143;
t205 = OP[1][0]*t196;
t199 = OP[3][0]+t198-t204-t205;
t200 = OP[0][1]*t147;
t206 = OP[2][1]*t143;
t207 = OP[1][1]*t196;
t201 = OP[3][1]+t200-t206-t207;
t202 = OP[0][2]*t147;
t208 = OP[2][2]*t143;
t209 = OP[1][2]*t196;
t203 = OP[3][2]+t202-t208-t209;
t210 = -t144+t197;
t211 = OP[1][0]*t210;
t212 = OP[3][0]+t198-t204+t211;
t213 = OP[1][1]*t210;
t214 = OP[3][1]+t200-t206+t213;
t215 = OP[1][2]*t210;
t216 = OP[3][2]+t202-t208+t215;
t217 = OP[0][6]*t147;
t218 = OP[0][7]*t147;
t219 = OP[0][8]*t147;
t220 = OP[1][0]*t155;
t221 = OP[2][0]*t149;
t229 = OP[0][0]*t152;
t222 = OP[4][0]+t220+t221-t229;
t223 = OP[1][1]*t155;
t224 = OP[2][1]*t149;
t230 = OP[0][1]*t152;
t225 = OP[4][1]+t223+t224-t230;
t226 = OP[1][2]*t155;
t227 = OP[2][2]*t149;
t231 = OP[0][2]*t152;
t228 = OP[4][2]+t226+t227-t231;
t232 = dvxNoise*t70*t84;
t233 = OP[1][6]*t155;
t234 = OP[2][6]*t149;
t235 = OP[4][6]+t233+t234-OP[0][6]*t152;
t236 = OP[1][7]*t155;
t237 = OP[2][7]*t149;
t238 = OP[4][7]+t236+t237-OP[0][7]*t152;
t239 = OP[1][8]*t155;
t240 = OP[2][8]*t149;
t241 = OP[4][8]+t239+t240-OP[0][8]*t152;
t242 = OP[2][0]*t163;
t243 = OP[0][0]*t160;
t251 = OP[1][0]*t158;
t244 = OP[5][0]+t242+t243-t251;
t245 = OP[2][1]*t163;
t246 = OP[0][1]*t160;
t252 = OP[1][1]*t158;
t247 = OP[5][1]+t245+t246-t252;
t248 = OP[2][2]*t163;
t249 = OP[0][2]*t160;
t253 = OP[1][2]*t158;
t250 = OP[5][2]+t248+t249-t253;
t254 = dvzNoise*t75*t85;
t255 = dvyNoise*t81*t87;
t256 = OP[2][6]*t163;
t257 = OP[0][6]*t160;
t258 = OP[5][6]+t256+t257-OP[1][6]*t158;
t259 = OP[2][7]*t163;
t260 = OP[0][7]*t160;
t261 = OP[5][7]+t259+t260-OP[1][7]*t158;
t262 = OP[2][8]*t163;
t263 = OP[0][8]*t160;
t264 = OP[5][8]+t262+t263-OP[1][8]*t158;
A0[0][0] = daxNoise*t122+t34*t61+t48*t68-t56*t69-t39*t91;
A0[0][0] = -t39*t94-t61*t101+t68*t105+t69*t109;
A0[0][1] = -t39*t97-t68*t134+t69*t140+t128*(t59+t60-t66-t67);
A0[0][2] = OP[0][3]*t34+OP[1][3]*t48-OP[2][3]*t56-OP[6][3]*t39-t69*t143+t147*(t59+t60-t66-t67)-t196*(t62+t63-t71-t72);
A0[0][3] = OP[0][4]*t34+OP[1][4]*t48-OP[2][4]*t56-OP[6][4]*t39-t61*t152+t69*t149+t155*(t62+t63-t71-t72);
A0[0][4] = OP[0][5]*t34+OP[1][5]*t48-OP[2][5]*t56-OP[6][5]*t39+t61*t160-t68*t158+t163*(t64+t65-t79-t80);
A0[0][5] = t91;
A0[0][6] = t94;
A0[0][7] = t97;
A0[0][0] = -t34*t115-t48*t118+t56*t121+t39*(t164+t165-OP[1][6]*t105-OP[2][6]*t109);
A0[0][0] = dayNoise*t122+t101*t115-t105*t118-t109*t121+t39*(t166+t167-OP[1][7]*t105-OP[2][7]*t109);
A0[0][1] = -t115*t128+t118*t134-t121*t140+t39*(t168+t169-OP[1][8]*t105-OP[2][8]*t109);
A0[0][2] = -OP[0][3]*t101-OP[7][3]*t39+OP[1][3]*t105+OP[2][3]*t109-t115*t147+t121*t143+t118*t196;
A0[0][3] = -OP[0][4]*t101-OP[7][4]*t39+OP[1][4]*t105+OP[2][4]*t109+t115*t152-t121*t149-t118*t155;
A0[0][4] = -OP[0][5]*t101-OP[7][5]*t39+OP[1][5]*t105+OP[2][5]*t109-t115*t160+t118*t158-t121*t163;
A0[0][5] = -t164-t165+OP[1][6]*t105+OP[2][6]*t109;
A0[0][6] = -t166-t167+OP[1][7]*t105+OP[2][7]*t109;
A0[0][7] = -t168-t169+OP[1][8]*t105+OP[2][8]*t109;
A0[1][0] = -t34*t175-t48*t178+t56*t181+t39*(t190+t191-OP[0][6]*t128-OP[2][6]*t140);
A0[1][0] = t101*t175-t105*t178-t109*t181+t39*(t192+t193-OP[0][7]*t128-OP[2][7]*t140);
A0[1][1] = dazNoise*t122-t128*t175+t134*t178-t140*t181+t39*(t194+t195-OP[0][8]*t128-OP[2][8]*t140);
A0[1][2] = -OP[8][3]*t39+OP[0][3]*t128-OP[1][3]*t134+OP[2][3]*t140-t147*t175+t143*t181+t178*t196;
A0[1][3] = -OP[8][4]*t39+OP[0][4]*t128-OP[1][4]*t134+OP[2][4]*t140+t152*t175-t149*t181-t155*t178;
A0[1][4] = -OP[8][5]*t39+OP[0][5]*t128-OP[1][5]*t134+OP[2][5]*t140-t160*t175+t158*t178-t163*t181;
A0[1][5] = -t190-t191+OP[0][6]*t128+OP[2][6]*t140;
A0[1][6] = -t192-t193+OP[0][7]*t128+OP[2][7]*t140;
A0[1][7] = -t194-t195+OP[0][8]*t128+OP[2][8]*t140;
A0[2][0] = -t39*(OP[3][6]+t217-OP[2][6]*t143-OP[1][6]*t196)+t34*t199+t48*t201-t56*t203;
A0[2][0] = -t39*(OP[3][7]+t218-OP[2][7]*t143-OP[1][7]*t196)-t101*t199+t105*t201+t109*t203;
A0[2][1] = -t39*(OP[3][8]+t219-OP[2][8]*t143-OP[1][8]*t196)+t128*t199-t134*t201+t140*t203;
A0[2][2] = OP[3][3]+OP[0][3]*t147-OP[2][3]*t143+OP[1][3]*t210-t143*t203+t147*t212+t210*t214+dvxNoise*(t70*t70)+dvyNoise*(t77*t77)+dvzNoise*(t75*t75);
A0[2][3] = OP[3][4]+t232+OP[0][4]*t147-OP[2][4]*t143+OP[1][4]*t210-t152*t212+t149*t216+t155*t214-dvyNoise*t77*t81-dvzNoise*t75*t83;
A0[2][4] = OP[3][5]+t254+OP[0][5]*t147-OP[2][5]*t143+OP[1][5]*t210-t158*t214+t160*t212+t163*t216-dvxNoise*t70*t88-dvyNoise*t77*t87;
A0[2][5] = OP[3][6]+t217-OP[2][6]*t143+OP[1][6]*t210;
A0[2][6] = OP[3][7]+t218-OP[2][7]*t143+OP[1][7]*t210;
A0[2][7] = OP[3][8]+t219-OP[2][8]*t143+OP[1][8]*t210;
A0[3][0] = t34*t222+t48*t225-t39*t235-t56*t228;
A0[3][0] = -t39*t238-t101*t222+t105*t225+t109*t228;
A0[3][1] = -t39*t241+t128*t222-t134*t225+t140*t228;
A0[3][2] = OP[4][3]+t232-OP[0][3]*t152+OP[1][3]*t155+OP[2][3]*t149+t147*t222-t143*t228+t210*t225-dvyNoise*t77*t81-dvzNoise*t75*t83;
A0[3][3] = OP[4][4]-OP[0][4]*t152+OP[1][4]*t155+OP[2][4]*t149-t152*t222+t149*t228+t155*t225+dvxNoise*(t84*t84)+dvyNoise*(t81*t81)+dvzNoise*(t83*t83);
A0[3][4] = OP[4][5]+t255-OP[0][5]*t152+OP[1][5]*t155+OP[2][5]*t149+t160*t222-t158*t225+t163*t228-dvxNoise*t84*t88-dvzNoise*t83*t85;
A0[3][5] = t235;
A0[3][6] = t238;
A0[3][7] = t241;
A0[4][0] = t34*t244+t48*t247-t39*t258-t56*t250;
A0[4][0] = -t39*t261-t101*t244+t105*t247+t109*t250;
A0[4][1] = -t39*t264+t128*t244-t134*t247+t140*t250;
A0[4][2] = OP[5][3]+t254+OP[0][3]*t160-OP[1][3]*t158+OP[2][3]*t163+t147*t244-t143*t250+t210*t247-dvxNoise*t70*t88-dvyNoise*t77*t87;
A0[4][3] = OP[5][4]+t255+OP[0][4]*t160-OP[1][4]*t158+OP[2][4]*t163-t152*t244+t149*t250+t155*t247-dvxNoise*t84*t88-dvzNoise*t83*t85;
A0[4][4] = OP[5][5]+OP[0][5]*t160-OP[1][5]*t158+OP[2][5]*t163+t160*t244-t158*t247+t163*t250+dvxNoise*(t88*t88)+dvyNoise*(t87*t87)+dvzNoise*(t85*t85);
A0[4][5] = t258;
A0[4][6] = t261;
A0[4][7] = t264;
A0[5][0] = -t265+OP[6][0]*t34+OP[6][1]*t48-OP[6][2]*t56;
A0[5][0] = -t266-OP[6][0]*t101+OP[6][1]*t105+OP[6][2]*t109;
A0[5][1] = -t267+OP[6][0]*t128-OP[6][1]*t134+OP[6][2]*t140;
A0[5][2] = OP[6][3]-OP[6][2]*t143+OP[6][0]*t147+OP[6][1]*t210;
A0[5][3] = OP[6][4]+OP[6][2]*t149-OP[6][0]*t152+OP[6][1]*t155;
A0[5][4] = OP[6][5]-OP[6][1]*t158+OP[6][0]*t160+OP[6][2]*t163;
A0[5][5] = OP[6][6];
A0[5][6] = OP[6][7];
A0[5][7] = OP[6][8];
A0[6][0] = -t164+OP[7][0]*t34+OP[7][1]*t48-OP[7][2]*t56;
A0[6][0] = -t166-OP[7][0]*t101+OP[7][1]*t105+OP[7][2]*t109;
A0[6][1] = -t168+OP[7][0]*t128-OP[7][1]*t134+OP[7][2]*t140;
A0[6][2] = OP[7][3]-OP[7][2]*t143+OP[7][0]*t147+OP[7][1]*t210;
A0[6][3] = OP[7][4]+OP[7][2]*t149-OP[7][0]*t152+OP[7][1]*t155;
A0[6][4] = OP[7][5]-OP[7][1]*t158+OP[7][0]*t160+OP[7][2]*t163;
A0[6][5] = OP[7][6];
A0[6][6] = OP[7][7];
A0[6][7] = OP[7][8];
A0[7][0] = -t190+OP[8][0]*t34+OP[8][1]*t48-OP[8][2]*t56;
A0[7][0] = -t192-OP[8][0]*t101+OP[8][1]*t105+OP[8][2]*t109;
A0[7][1] = -t194+OP[8][0]*t128-OP[8][1]*t134+OP[8][2]*t140;
A0[7][2] = OP[8][3]-OP[8][2]*t143+OP[8][0]*t147+OP[8][1]*t210;
A0[7][3] = OP[8][4]+OP[8][2]*t149-OP[8][0]*t152+OP[8][1]*t155;
A0[7][4] = OP[8][5]-OP[8][1]*t158+OP[8][0]*t160+OP[8][2]*t163;
A0[7][5] = OP[8][6];
A0[7][6] = OP[8][7];
A0[7][7] = OP[8][8];
Reference in New Issue View Git Blame Copy Permalink