asctec_hl_firmware: autogen_ekf_propagation.c Source File

Go to the documentation of this file.
00001 /*
00002  * autogen_ekf_propagation.c
00003  *
00004  * Embedded MATLAB Coder code generation for function 'autogen_ekf_propagation'
00005  *
00006  * C source code generated on: Wed Aug 24 18:34:48 2011
00007  *
00008  */
00009 
00010 /* Include files */
00011 #include "rt_nonfinite.h"
00012 #include "autogen_ekf_propagation.h"
00013 
00014 /* Type Definitions */
00015 
00016 /* Named Constants */
00017 
00018 /* Variable Declarations */
00019 
00020 /* Variable Definitions */
00021 
00022 /* Function Declarations */
00023 
00024 /* Function Definitions */
00025 void autogen_ekf_propagation(const real32_T eml_in1[20], const real32_T eml_in2
00026   [3], const real32_T eml_in3[3], real32_T eml_dt,
00027   real32_T eml_x_new[16])
00028 {
00029   real32_T eml_t17;
00030   real32_T eml_t18;
00031   real32_T eml_t19;
00032   real32_T eml_t20;
00033   real32_T eml_t21;
00034   real32_T eml_t22;
00035   real32_T eml_t23;
00036   real32_T eml_t24;
00037   real32_T eml_t25;
00038   real32_T eml_t26;
00039   real32_T eml_t27;
00040   real32_T eml_t28;
00041   real32_T eml_t29;
00042   real32_T eml_t30;
00043 
00044   /* AUTOGEN_EKF_PROPAGATION */
00045   /*     X_NEW = AUTOGEN_EKF_PROPAGATION(IN1,IN2,IN3,DT) */
00046   /*     This function was generated by the Symbolic Math Toolbox version 5.5. */
00047   /*     24-Aug-2011 18:34:47 */
00048   eml_t17 = eml_in2[1] - eml_in1[14];
00049   eml_t18 = rt_pow32_snf(eml_in1[6], 2.0F);
00050   eml_t19 = rt_pow32_snf(eml_in1[7], 2.0F);
00051   eml_t20 = rt_pow32_snf(eml_in1[8], 2.0F);
00052   eml_t21 = rt_pow32_snf(eml_in1[9], 2.0F);
00053   eml_t22 = eml_in2[0] - eml_in1[13];
00054   eml_t23 = eml_in1[7] * eml_in1[8] * 2.0F;
00055   eml_t24 = eml_in2[2] - eml_in1[15];
00056   eml_t25 = eml_in1[6] * eml_in1[8] * 2.0F;
00057   eml_t26 = eml_in1[7] * eml_in1[9] * 2.0F;
00058   eml_t27 = eml_in1[6] * eml_in1[7] * 2.0F;
00059   eml_t28 = eml_in3[0] - eml_in1[10];
00060   eml_t29 = eml_in3[1] - eml_in1[11];
00061   eml_t30 = eml_in3[2] - eml_in1[12];
00062   eml_x_new[0] = eml_in1[0] + eml_dt * eml_in1[3];
00063   eml_x_new[1] = eml_in1[1] + eml_dt * eml_in1[4];
00064   eml_x_new[2] = eml_in1[2] + eml_dt * eml_in1[5];
00065   eml_x_new[3] = eml_in1[3] + eml_dt * ((eml_t24 * (eml_t25 + eml_t26) + eml_t22
00066     * (((eml_t18 + eml_t19) - eml_t20) - eml_t21)) +
00067     eml_t17 * (eml_t23 - eml_in1[6] * eml_in1[9] * 2.0F));
00068   eml_x_new[4] = eml_in1[4] + eml_dt * ((eml_t17 * (((eml_t18 - eml_t19) +
00069     eml_t20) - eml_t21) + eml_t22 * (eml_t23 + eml_in1[6]
00070     * eml_in1[9] * 2.0F)) - eml_t24 * (eml_t27 - eml_in1[8] * eml_in1[9] * 2.0F));
00071   eml_x_new[5] = eml_in1[5] - eml_dt * (((eml_t22 * (eml_t25 - eml_t26) -
00072     eml_t24 * (((eml_t18 - eml_t19) - eml_t20) + eml_t21))
00073     - eml_t17 * (eml_t27 + eml_in1[8] * eml_in1[9] * 2.0F)) + 9.81F);
00074   eml_x_new[6] = ((eml_in1[6] - eml_dt * eml_t28 * eml_in1[7] * 0.5F) - eml_dt *
00075                   eml_t29 * eml_in1[8] * 0.5F) - eml_dt * eml_t30
00076     * eml_in1[9] * 0.5F;
00077   eml_x_new[7] = ((eml_in1[7] + eml_dt * eml_t28 * eml_in1[6] * 0.5F) - eml_dt *
00078                   eml_t29 * eml_in1[9] * 0.5F) + eml_dt * eml_t30
00079     * eml_in1[8] * 0.5F;
00080   eml_x_new[8] = ((eml_in1[8] + eml_dt * eml_t28 * eml_in1[9] * 0.5F) + eml_dt *
00081                   eml_t29 * eml_in1[6] * 0.5F) - eml_dt * eml_t30
00082     * eml_in1[7] * 0.5F;
00083   eml_x_new[9] = ((eml_in1[9] - eml_dt * eml_t28 * eml_in1[8] * 0.5F) + eml_dt *
00084                   eml_t29 * eml_in1[7] * 0.5F) + eml_dt * eml_t30
00085     * eml_in1[6] * 0.5F;
00086   eml_x_new[10] = eml_in1[10];
00087   eml_x_new[11] = eml_in1[11];
00088   eml_x_new[12] = eml_in1[12];
00089   eml_x_new[13] = eml_in1[13];
00090   eml_x_new[14] = eml_in1[14];
00091   eml_x_new[15] = eml_in1[15];
00092 }
00093 
00094 /* End of Embedded MATLAB Coder code generation (autogen_ekf_propagation.c) */