diff options
Diffstat (limited to 'apps/position_estimator/codegen/position_estimator.c')
-rw-r--r-- | apps/position_estimator/codegen/position_estimator.c | 261 |
1 files changed, 0 insertions, 261 deletions
diff --git a/apps/position_estimator/codegen/position_estimator.c b/apps/position_estimator/codegen/position_estimator.c deleted file mode 100644 index 731ae03e3..000000000 --- a/apps/position_estimator/codegen/position_estimator.c +++ /dev/null @@ -1,261 +0,0 @@ -/* - * position_estimator.c - * - * Code generation for function 'position_estimator' - * - * C source code generated on: Fri Jun 8 13:31:21 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "position_estimator.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T - xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T - predict_only, real32_T xapo1[6], real32_T Papo1[36]) -{ - real32_T fv0[6]; - real32_T fv1[6]; - real32_T I[36]; - real32_T xapri[6]; - int32_T i; - int32_T r1; - static const real32_T fv2[36] = { 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.004F, - 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.004F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.004F, 1.0F }; - - static const real32_T fv3[12] = { 0.0F, 0.0F, 0.1744F, 87.2F, 0.0F, 0.0F, - -0.1744F, -87.2F, 0.0F, 0.0F, 0.0F, 0.0F }; - - int32_T r2; - real32_T Papri[36]; - real32_T maxval; - static const real32_T fv4[36] = { 1.0F, 0.004F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F }; - - static const real32_T fv5[36] = { 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F }; - - real32_T K[18]; - static const int8_T iv0[18] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0 }; - - real32_T fv6[9]; - static const int8_T iv1[18] = { 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 1, 0 }; - - real32_T b_gps_covariance[9]; - real32_T A[9]; - real32_T B[18]; - int32_T r3; - real32_T a21; - real32_T Y[18]; - real32_T b_z[3]; - int8_T b_I[36]; - - /* if predit_onli == 1: no update step: use this when no new gps data is available */ - /* %initialization */ - /* use model F=m*a x''=F/m */ - /* 250Hz---> dT = 0.004s */ - /* u=[phi;theta] */ - /* x=[px;vx;py;vy]; */ - /* %------------------------------------------ */ - /* %------------------------------------------------ */ - /* R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1]; */ - /* process Covariance Matrix */ - /* measurement Covariance Matrix */ - /* %prediction */ - for (i = 0; i < 6; i++) { - fv0[i] = 0.0F; - for (r1 = 0; r1 < 6; r1++) { - fv0[i] += fv2[i + 6 * r1] * xapo[r1]; - } - - fv1[i] = 0.0F; - for (r1 = 0; r1 < 2; r1++) { - fv1[i] += fv3[i + 6 * r1] * u[r1]; - } - - xapri[i] = fv0[i] + fv1[i]; - for (r1 = 0; r1 < 6; r1++) { - I[i + 6 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - I[i + 6 * r1] += fv2[i + 6 * r2] * Papo[r2 + 6 * r1]; - } - } - } - - for (i = 0; i < 6; i++) { - for (r1 = 0; r1 < 6; r1++) { - maxval = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - maxval += I[i + 6 * r2] * fv4[r2 + 6 * r1]; - } - - Papri[i + 6 * r1] = maxval + fv5[i + 6 * r1]; - } - } - - if (1 != predict_only) { - /* update */ - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 6; r1++) { - K[i + 3 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - K[i + 3 * r1] += (real32_T)iv0[i + 3 * r2] * Papri[r2 + 6 * r1]; - } - } - } - - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 3; r1++) { - fv6[i + 3 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - fv6[i + 3 * r1] += K[r1 + 3 * r2] * (real32_T)iv1[r2 + 6 * i]; - } - } - } - - b_gps_covariance[0] = gps_covariance[0]; - b_gps_covariance[1] = 0.0F; - b_gps_covariance[2] = 0.0F; - b_gps_covariance[3] = 0.0F; - b_gps_covariance[4] = gps_covariance[1]; - b_gps_covariance[5] = 0.0F; - b_gps_covariance[6] = 0.0F; - b_gps_covariance[7] = 0.0F; - b_gps_covariance[8] = gps_covariance[2]; - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 3; r1++) { - A[r1 + 3 * i] = fv6[r1 + 3 * i] + b_gps_covariance[r1 + 3 * i]; - } - - for (r1 = 0; r1 < 6; r1++) { - B[i + 3 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - B[i + 3 * r1] += Papri[r1 + 6 * r2] * (real32_T)iv1[r2 + 6 * i]; - } - } - } - - r1 = 0; - r2 = 1; - r3 = 2; - maxval = (real32_T)fabs(A[0]); - a21 = (real32_T)fabs(A[1]); - if (a21 > maxval) { - maxval = a21; - r1 = 1; - r2 = 0; - } - - if ((real32_T)fabs(A[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } - - A[r2] /= A[r1]; - A[r3] /= A[r1]; - A[3 + r2] -= A[r2] * A[3 + r1]; - A[3 + r3] -= A[r3] * A[3 + r1]; - A[6 + r2] -= A[r2] * A[6 + r1]; - A[6 + r3] -= A[r3] * A[6 + r1]; - if ((real32_T)fabs(A[3 + r3]) > (real32_T)fabs(A[3 + r2])) { - i = r2; - r2 = r3; - r3 = i; - } - - A[3 + r3] /= A[3 + r2]; - A[6 + r3] -= A[3 + r3] * A[6 + r2]; - for (i = 0; i < 6; i++) { - Y[3 * i] = B[r1 + 3 * i]; - Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * A[r2]; - Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * A[r3]) - Y[1 + 3 * i] * A[3 + - r3]; - Y[2 + 3 * i] /= A[6 + r3]; - Y[3 * i] -= Y[2 + 3 * i] * A[6 + r1]; - Y[1 + 3 * i] -= Y[2 + 3 * i] * A[6 + r2]; - Y[1 + 3 * i] /= A[3 + r2]; - Y[3 * i] -= Y[1 + 3 * i] * A[3 + r1]; - Y[3 * i] /= A[r1]; - } - - for (i = 0; i < 3; i++) { - for (r1 = 0; r1 < 6; r1++) { - K[r1 + 6 * i] = Y[i + 3 * r1]; - } - } - - for (i = 0; i < 3; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 6; r1++) { - maxval += (real32_T)iv0[i + 3 * r1] * xapri[r1]; - } - - b_z[i] = z[i] - maxval; - } - - for (i = 0; i < 6; i++) { - maxval = 0.0F; - for (r1 = 0; r1 < 3; r1++) { - maxval += K[i + 6 * r1] * b_z[r1]; - } - - xapo1[i] = xapri[i] + maxval; - } - - for (i = 0; i < 36; i++) { - b_I[i] = 0; - } - - for (i = 0; i < 6; i++) { - b_I[i + 6 * i] = 1; - } - - for (i = 0; i < 6; i++) { - for (r1 = 0; r1 < 6; r1++) { - maxval = 0.0F; - for (r2 = 0; r2 < 3; r2++) { - maxval += K[i + 6 * r2] * (real32_T)iv0[r2 + 3 * r1]; - } - - I[i + 6 * r1] = (real32_T)b_I[i + 6 * r1] - maxval; - } - } - - for (i = 0; i < 6; i++) { - for (r1 = 0; r1 < 6; r1++) { - Papo1[i + 6 * r1] = 0.0F; - for (r2 = 0; r2 < 6; r2++) { - Papo1[i + 6 * r1] += I[i + 6 * r2] * Papri[r2 + 6 * r1]; - } - } - } - } else { - memcpy((void *)&Papo1[0], (void *)&Papri[0], 36U * sizeof(real32_T)); - for (i = 0; i < 6; i++) { - xapo1[i] = xapri[i]; - } - } -} - -/* End of code generation (position_estimator.c) */ |