From e6ed8268ee610d0b9e9b4930ad379a6d7dcc3629 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 12:59:47 +0200 Subject: Moved position_estimator_mc, px4io driver and sdlog app to new style build --- .../position_estimator_mc/codegen/kalman_dlqe1.c | 58 +++ .../position_estimator_mc/codegen/kalman_dlqe1.h | 30 ++ .../codegen/kalman_dlqe1_initialize.c | 31 ++ .../codegen/kalman_dlqe1_initialize.h | 30 ++ .../codegen/kalman_dlqe1_terminate.c | 31 ++ .../codegen/kalman_dlqe1_terminate.h | 30 ++ .../codegen/kalman_dlqe1_types.h | 16 + .../position_estimator_mc/codegen/kalman_dlqe2.c | 119 +++++ .../position_estimator_mc/codegen/kalman_dlqe2.h | 32 ++ .../codegen/kalman_dlqe2_initialize.c | 31 ++ .../codegen/kalman_dlqe2_initialize.h | 32 ++ .../codegen/kalman_dlqe2_terminate.c | 31 ++ .../codegen/kalman_dlqe2_terminate.h | 32 ++ .../codegen/kalman_dlqe2_types.h | 16 + .../position_estimator_mc/codegen/kalman_dlqe3.c | 137 ++++++ .../position_estimator_mc/codegen/kalman_dlqe3.h | 33 ++ .../codegen/kalman_dlqe3_data.c | 32 ++ .../codegen/kalman_dlqe3_data.h | 38 ++ .../codegen/kalman_dlqe3_initialize.c | 47 ++ .../codegen/kalman_dlqe3_initialize.h | 33 ++ .../codegen/kalman_dlqe3_terminate.c | 31 ++ .../codegen/kalman_dlqe3_terminate.h | 33 ++ .../codegen/kalman_dlqe3_types.h | 16 + .../codegen/positionKalmanFilter1D.c | 136 ++++++ .../codegen/positionKalmanFilter1D.h | 31 ++ .../codegen/positionKalmanFilter1D_dT.c | 157 ++++++ .../codegen/positionKalmanFilter1D_dT.h | 31 ++ .../codegen/positionKalmanFilter1D_dT_initialize.c | 31 ++ .../codegen/positionKalmanFilter1D_dT_initialize.h | 31 ++ .../codegen/positionKalmanFilter1D_dT_terminate.c | 31 ++ .../codegen/positionKalmanFilter1D_dT_terminate.h | 31 ++ .../codegen/positionKalmanFilter1D_dT_types.h | 16 + .../codegen/positionKalmanFilter1D_initialize.c | 31 ++ .../codegen/positionKalmanFilter1D_initialize.h | 31 ++ .../codegen/positionKalmanFilter1D_terminate.c | 31 ++ .../codegen/positionKalmanFilter1D_terminate.h | 31 ++ .../codegen/positionKalmanFilter1D_types.h | 16 + src/modules/position_estimator_mc/codegen/randn.c | 524 +++++++++++++++++++++ src/modules/position_estimator_mc/codegen/randn.h | 33 ++ .../position_estimator_mc/codegen/rtGetInf.c | 139 ++++++ .../position_estimator_mc/codegen/rtGetInf.h | 23 + .../position_estimator_mc/codegen/rtGetNaN.c | 96 ++++ .../position_estimator_mc/codegen/rtGetNaN.h | 21 + .../position_estimator_mc/codegen/rt_nonfinite.c | 87 ++++ .../position_estimator_mc/codegen/rt_nonfinite.h | 53 +++ .../position_estimator_mc/codegen/rtwtypes.h | 159 +++++++ 46 files changed, 2689 insertions(+) create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1.c create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2.c create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3.c create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h create mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h create mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h create mode 100755 src/modules/position_estimator_mc/codegen/randn.c create mode 100755 src/modules/position_estimator_mc/codegen/randn.h create mode 100755 src/modules/position_estimator_mc/codegen/rtGetInf.c create mode 100755 src/modules/position_estimator_mc/codegen/rtGetInf.h create mode 100755 src/modules/position_estimator_mc/codegen/rtGetNaN.c create mode 100755 src/modules/position_estimator_mc/codegen/rtGetNaN.h create mode 100755 src/modules/position_estimator_mc/codegen/rt_nonfinite.c create mode 100755 src/modules/position_estimator_mc/codegen/rt_nonfinite.h create mode 100755 src/modules/position_estimator_mc/codegen/rtwtypes.h (limited to 'src/modules/position_estimator_mc/codegen') diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c new file mode 100755 index 000000000..977565b8e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c @@ -0,0 +1,58 @@ +/* + * kalman_dlqe1.c + * + * Code generation for function 'kalman_dlqe1' + * + * C source code generated on: Wed Feb 13 20:34:32 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe1.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], + const real32_T x_aposteriori_k[3], real32_T z, real32_T + x_aposteriori[3]) +{ + printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z); + printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2])); + real32_T y; + int32_T i0; + real32_T b_y[3]; + int32_T i1; + real32_T f0; + y = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + b_y[i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_y[i0] += C[i1] * A[i1 + 3 * i0]; + } + + y += b_y[i0] * x_aposteriori_k[i0]; + } + + y = z - y; + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; + } + + x_aposteriori[i0] = f0 + K[i0] * y; + } + //printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2])); +} + +/* End of code generation (kalman_dlqe1.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h new file mode 100755 index 000000000..2f5330563 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h @@ -0,0 +1,30 @@ +/* + * kalman_dlqe1.h + * + * Code generation for function 'kalman_dlqe1' + * + * C source code generated on: Wed Feb 13 20:34:32 2013 + * + */ + +#ifndef __KALMAN_DLQE1_H__ +#define __KALMAN_DLQE1_H__ +/* Include files */ +#include +#include + +#include "rtwtypes.h" +#include "kalman_dlqe1_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]); +#endif +/* End of code generation (kalman_dlqe1.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c new file mode 100755 index 000000000..6627f76e9 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c @@ -0,0 +1,31 @@ +/* + * kalman_dlqe1_initialize.c + * + * Code generation for function 'kalman_dlqe1_initialize' + * + * C source code generated on: Wed Feb 13 20:34:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe1.h" +#include "kalman_dlqe1_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe1_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (kalman_dlqe1_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h new file mode 100755 index 000000000..a77eb5712 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h @@ -0,0 +1,30 @@ +/* + * kalman_dlqe1_initialize.h + * + * Code generation for function 'kalman_dlqe1_initialize' + * + * C source code generated on: Wed Feb 13 20:34:31 2013 + * + */ + +#ifndef __KALMAN_DLQE1_INITIALIZE_H__ +#define __KALMAN_DLQE1_INITIALIZE_H__ +/* Include files */ +#include +#include + +#include "rtwtypes.h" +#include "kalman_dlqe1_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe1_initialize(void); +#endif +/* End of code generation (kalman_dlqe1_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c new file mode 100755 index 000000000..a65536f79 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c @@ -0,0 +1,31 @@ +/* + * kalman_dlqe1_terminate.c + * + * Code generation for function 'kalman_dlqe1_terminate' + * + * C source code generated on: Wed Feb 13 20:34:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe1.h" +#include "kalman_dlqe1_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe1_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (kalman_dlqe1_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h new file mode 100755 index 000000000..100c7f76c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h @@ -0,0 +1,30 @@ +/* + * kalman_dlqe1_terminate.h + * + * Code generation for function 'kalman_dlqe1_terminate' + * + * C source code generated on: Wed Feb 13 20:34:32 2013 + * + */ + +#ifndef __KALMAN_DLQE1_TERMINATE_H__ +#define __KALMAN_DLQE1_TERMINATE_H__ +/* Include files */ +#include +#include + +#include "rtwtypes.h" +#include "kalman_dlqe1_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe1_terminate(void); +#endif +/* End of code generation (kalman_dlqe1_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h new file mode 100755 index 000000000..d4b2c2d61 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h @@ -0,0 +1,16 @@ +/* + * kalman_dlqe1_types.h + * + * Code generation for function 'kalman_dlqe1' + * + * C source code generated on: Wed Feb 13 20:34:31 2013 + * + */ + +#ifndef __KALMAN_DLQE1_TYPES_H__ +#define __KALMAN_DLQE1_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (kalman_dlqe1_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c new file mode 100755 index 000000000..11b999064 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c @@ -0,0 +1,119 @@ +/* + * kalman_dlqe2.c + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe2.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +static real32_T rt_powf_snf(real32_T u0, real32_T u1); + +/* Function Definitions */ +static real32_T rt_powf_snf(real32_T u0, real32_T u1) +{ + real32_T y; + real32_T f1; + real32_T f2; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else { + f1 = (real32_T)fabs(u0); + f2 = (real32_T)fabs(u1); + if (rtIsInfF(u1)) { + if (f1 == 1.0F) { + y = ((real32_T)rtNaN); + } else if (f1 > 1.0F) { + if (u1 > 0.0F) { + y = ((real32_T)rtInf); + } else { + y = 0.0F; + } + } else if (u1 > 0.0F) { + y = 0.0F; + } else { + y = ((real32_T)rtInf); + } + } else if (f2 == 0.0F) { + y = 1.0F; + } else if (f2 == 1.0F) { + if (u1 > 0.0F) { + y = u0; + } else { + y = 1.0F / u0; + } + } else if (u1 == 2.0F) { + y = u0 * u0; + } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { + y = (real32_T)sqrt(u0); + } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { + y = ((real32_T)rtNaN); + } else { + y = (real32_T)pow(u0, u1); + } + } + + return y; +} + +void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const + real32_T x_aposteriori_k[3], real32_T z, real32_T + x_aposteriori[3]) +{ + //printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3)); + //printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3)); + real32_T A[9]; + real32_T y; + int32_T i0; + static const int8_T iv0[3] = { 0, 0, 1 }; + + real32_T b_k1[3]; + int32_T i1; + static const int8_T iv1[3] = { 1, 0, 0 }; + + real32_T f0; + A[0] = 1.0F; + A[3] = dt; + A[6] = 0.5F * rt_powf_snf(dt, 2.0F); + A[1] = 0.0F; + A[4] = 1.0F; + A[7] = dt; + y = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + A[2 + 3 * i0] = (real32_T)iv0[i0]; + b_k1[i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0]; + } + + y += b_k1[i0] * x_aposteriori_k[i0]; + } + + y = z - y; + b_k1[0] = k1; + b_k1[1] = k2; + b_k1[2] = k3; + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; + } + + x_aposteriori[i0] = f0 + b_k1[i0] * y; + } +} + +/* End of code generation (kalman_dlqe2.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h new file mode 100755 index 000000000..30170ae22 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h @@ -0,0 +1,32 @@ +/* + * kalman_dlqe2.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +#ifndef __KALMAN_DLQE2_H__ +#define __KALMAN_DLQE2_H__ +/* Include files */ +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe2_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]); +#endif +/* End of code generation (kalman_dlqe2.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c new file mode 100755 index 000000000..de5a1d8aa --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c @@ -0,0 +1,31 @@ +/* + * kalman_dlqe2_initialize.c + * + * Code generation for function 'kalman_dlqe2_initialize' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe2.h" +#include "kalman_dlqe2_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe2_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (kalman_dlqe2_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h new file mode 100755 index 000000000..3d507ff31 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h @@ -0,0 +1,32 @@ +/* + * kalman_dlqe2_initialize.h + * + * Code generation for function 'kalman_dlqe2_initialize' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +#ifndef __KALMAN_DLQE2_INITIALIZE_H__ +#define __KALMAN_DLQE2_INITIALIZE_H__ +/* Include files */ +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe2_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe2_initialize(void); +#endif +/* End of code generation (kalman_dlqe2_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c new file mode 100755 index 000000000..0757c878c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c @@ -0,0 +1,31 @@ +/* + * kalman_dlqe2_terminate.c + * + * Code generation for function 'kalman_dlqe2_terminate' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe2.h" +#include "kalman_dlqe2_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe2_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (kalman_dlqe2_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h new file mode 100755 index 000000000..23995020b --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h @@ -0,0 +1,32 @@ +/* + * kalman_dlqe2_terminate.h + * + * Code generation for function 'kalman_dlqe2_terminate' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +#ifndef __KALMAN_DLQE2_TERMINATE_H__ +#define __KALMAN_DLQE2_TERMINATE_H__ +/* Include files */ +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe2_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe2_terminate(void); +#endif +/* End of code generation (kalman_dlqe2_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h new file mode 100755 index 000000000..f7a04d908 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h @@ -0,0 +1,16 @@ +/* + * kalman_dlqe2_types.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:28 2013 + * + */ + +#ifndef __KALMAN_DLQE2_TYPES_H__ +#define __KALMAN_DLQE2_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (kalman_dlqe2_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c new file mode 100755 index 000000000..9efe2ea7a --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c @@ -0,0 +1,137 @@ +/* + * kalman_dlqe3.c + * + * Code generation for function 'kalman_dlqe3' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe3.h" +#include "randn.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +static real32_T rt_powf_snf(real32_T u0, real32_T u1); + +/* Function Definitions */ +static real32_T rt_powf_snf(real32_T u0, real32_T u1) +{ + real32_T y; + real32_T f1; + real32_T f2; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else { + f1 = (real32_T)fabs(u0); + f2 = (real32_T)fabs(u1); + if (rtIsInfF(u1)) { + if (f1 == 1.0F) { + y = ((real32_T)rtNaN); + } else if (f1 > 1.0F) { + if (u1 > 0.0F) { + y = ((real32_T)rtInf); + } else { + y = 0.0F; + } + } else if (u1 > 0.0F) { + y = 0.0F; + } else { + y = ((real32_T)rtInf); + } + } else if (f2 == 0.0F) { + y = 1.0F; + } else if (f2 == 1.0F) { + if (u1 > 0.0F) { + y = u0; + } else { + y = 1.0F / u0; + } + } else if (u1 == 2.0F) { + y = u0 * u0; + } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { + y = (real32_T)sqrt(u0); + } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { + y = ((real32_T)rtNaN); + } else { + y = (real32_T)pow(u0, u1); + } + } + + return y; +} + +void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const + real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, + real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]) +{ + real32_T A[9]; + int32_T i0; + static const int8_T iv0[3] = { 0, 0, 1 }; + + real_T b; + real32_T y; + real32_T b_y[3]; + int32_T i1; + static const int8_T iv1[3] = { 1, 0, 0 }; + + real32_T b_k1[3]; + real32_T f0; + A[0] = 1.0F; + A[3] = dt; + A[6] = 0.5F * rt_powf_snf(dt, 2.0F); + A[1] = 0.0F; + A[4] = 1.0F; + A[7] = dt; + for (i0 = 0; i0 < 3; i0++) { + A[2 + 3 * i0] = (real32_T)iv0[i0]; + } + + if (addNoise == 1.0F) { + b = randn(); + z += sigma * (real32_T)b; + } + + if (posUpdate != 0.0F) { + y = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + b_y[i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0]; + } + + y += b_y[i0] * x_aposteriori_k[i0]; + } + + y = z - y; + b_k1[0] = k1; + b_k1[1] = k2; + b_k1[2] = k3; + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; + } + + x_aposteriori[i0] = f0 + b_k1[i0] * y; + } + } else { + for (i0 = 0; i0 < 3; i0++) { + x_aposteriori[i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1]; + } + } + } +} + +/* End of code generation (kalman_dlqe3.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h new file mode 100755 index 000000000..9bbffbbb3 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h @@ -0,0 +1,33 @@ +/* + * kalman_dlqe3.h + * + * Code generation for function 'kalman_dlqe3' + * + * C source code generated on: Tue Feb 19 15:26:32 2013 + * + */ + +#ifndef __KALMAN_DLQE3_H__ +#define __KALMAN_DLQE3_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe3_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]); +#endif +/* End of code generation (kalman_dlqe3.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c new file mode 100755 index 000000000..8f2275c13 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c @@ -0,0 +1,32 @@ +/* + * kalman_dlqe3_data.c + * + * Code generation for function 'kalman_dlqe3_data' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe3.h" +#include "kalman_dlqe3_data.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ +uint32_T method; +uint32_T state[2]; +uint32_T b_method; +uint32_T b_state; +uint32_T c_state[2]; +boolean_T state_not_empty; + +/* Function Declarations */ + +/* Function Definitions */ +/* End of code generation (kalman_dlqe3_data.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h new file mode 100755 index 000000000..952eb7b89 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h @@ -0,0 +1,38 @@ +/* + * kalman_dlqe3_data.h + * + * Code generation for function 'kalman_dlqe3_data' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +#ifndef __KALMAN_DLQE3_DATA_H__ +#define __KALMAN_DLQE3_DATA_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe3_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ +extern uint32_T method; +extern uint32_T state[2]; +extern uint32_T b_method; +extern uint32_T b_state; +extern uint32_T c_state[2]; +extern boolean_T state_not_empty; + +/* Variable Definitions */ + +/* Function Declarations */ +#endif +/* End of code generation (kalman_dlqe3_data.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c new file mode 100755 index 000000000..b87d604c4 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c @@ -0,0 +1,47 @@ +/* + * kalman_dlqe3_initialize.c + * + * Code generation for function 'kalman_dlqe3_initialize' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe3.h" +#include "kalman_dlqe3_initialize.h" +#include "kalman_dlqe3_data.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe3_initialize(void) +{ + int32_T i; + static const uint32_T uv0[2] = { 362436069U, 0U }; + + rt_InitInfAndNaN(8U); + state_not_empty = FALSE; + b_state = 1144108930U; + b_method = 7U; + method = 0U; + for (i = 0; i < 2; i++) { + c_state[i] = 362436069U + 158852560U * (uint32_T)i; + state[i] = uv0[i]; + } + + if (state[1] == 0U) { + state[1] = 521288629U; + } +} + +/* End of code generation (kalman_dlqe3_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h new file mode 100755 index 000000000..9dee90f9e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h @@ -0,0 +1,33 @@ +/* + * kalman_dlqe3_initialize.h + * + * Code generation for function 'kalman_dlqe3_initialize' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +#ifndef __KALMAN_DLQE3_INITIALIZE_H__ +#define __KALMAN_DLQE3_INITIALIZE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe3_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe3_initialize(void); +#endif +/* End of code generation (kalman_dlqe3_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c new file mode 100755 index 000000000..b00858205 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c @@ -0,0 +1,31 @@ +/* + * kalman_dlqe3_terminate.c + * + * Code generation for function 'kalman_dlqe3_terminate' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe3.h" +#include "kalman_dlqe3_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void kalman_dlqe3_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (kalman_dlqe3_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h new file mode 100755 index 000000000..69cc85c76 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h @@ -0,0 +1,33 @@ +/* + * kalman_dlqe3_terminate.h + * + * Code generation for function 'kalman_dlqe3_terminate' + * + * C source code generated on: Tue Feb 19 15:26:31 2013 + * + */ + +#ifndef __KALMAN_DLQE3_TERMINATE_H__ +#define __KALMAN_DLQE3_TERMINATE_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe3_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void kalman_dlqe3_terminate(void); +#endif +/* End of code generation (kalman_dlqe3_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h new file mode 100755 index 000000000..f36ea4557 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h @@ -0,0 +1,16 @@ +/* + * kalman_dlqe3_types.h + * + * Code generation for function 'kalman_dlqe3' + * + * C source code generated on: Tue Feb 19 15:26:30 2013 + * + */ + +#ifndef __KALMAN_DLQE3_TYPES_H__ +#define __KALMAN_DLQE3_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (kalman_dlqe3_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c new file mode 100755 index 000000000..5139848bc --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c @@ -0,0 +1,136 @@ +/* + * positionKalmanFilter1D.c + * + * Code generation for function 'positionKalmanFilter1D' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const + real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T + P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T + Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], + real32_T P_aposteriori[9]) +{ + int32_T i0; + real32_T f0; + int32_T k; + real32_T b_A[9]; + int32_T i1; + real32_T P_apriori[9]; + real32_T y; + real32_T K[3]; + real32_T S; + int8_T I[9]; + + /* prediction */ + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (k = 0; k < 3; k++) { + f0 += A[i0 + 3 * k] * x_aposteriori_k[k]; + } + + x_aposteriori[i0] = f0 + B[i0] * u; + } + + for (i0 = 0; i0 < 3; i0++) { + for (k = 0; k < 3; k++) { + b_A[i0 + 3 * k] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + for (k = 0; k < 3; k++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1]; + } + + P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k]; + } + } + + if ((real32_T)fabs(u) < thresh) { + x_aposteriori[1] *= decay; + } + + /* update */ + if (gps_update == 1) { + y = 0.0F; + for (k = 0; k < 3; k++) { + y += C[k] * x_aposteriori[k]; + K[k] = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + K[k] += C[i0] * P_apriori[i0 + 3 * k]; + } + } + + y = z - y; + S = 0.0F; + for (k = 0; k < 3; k++) { + S += K[k] * C[k]; + } + + S += R; + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (k = 0; k < 3; k++) { + f0 += P_apriori[i0 + 3 * k] * C[k]; + } + + K[i0] = f0 / S; + } + + for (i0 = 0; i0 < 3; i0++) { + x_aposteriori[i0] += K[i0] * y; + } + + for (i0 = 0; i0 < 9; i0++) { + I[i0] = 0; + } + + for (k = 0; k < 3; k++) { + I[k + 3 * k] = 1; + } + + for (i0 = 0; i0 < 3; i0++) { + for (k = 0; k < 3; k++) { + b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0]; + } + } + + for (i0 = 0; i0 < 3; i0++) { + for (k = 0; k < 3; k++) { + P_aposteriori[i0 + 3 * k] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k]; + } + } + } + } else { + for (i0 = 0; i0 < 9; i0++) { + P_aposteriori[i0] = P_apriori[i0]; + } + } +} + +/* End of code generation (positionKalmanFilter1D.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h new file mode 100755 index 000000000..205c8eb4e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D.h + * + * Code generation for function 'positionKalmanFilter1D' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_H__ +#define __POSITIONKALMANFILTER1D_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]); +#endif +/* End of code generation (positionKalmanFilter1D.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c new file mode 100755 index 000000000..4c535618a --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c @@ -0,0 +1,157 @@ +/* + * positionKalmanFilter1D_dT.c + * + * Code generation for function 'positionKalmanFilter1D_dT' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D_dT.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], + const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, + const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T + x_aposteriori[3], real32_T P_aposteriori[9]) +{ + real32_T A[9]; + int32_T i; + static const int8_T iv0[3] = { 0, 0, 1 }; + + real32_T K[3]; + real32_T f0; + int32_T i0; + real32_T b_A[9]; + int32_T i1; + real32_T P_apriori[9]; + static const int8_T iv1[3] = { 1, 0, 0 }; + + real32_T fv0[3]; + real32_T y; + static const int8_T iv2[3] = { 1, 0, 0 }; + + real32_T S; + int8_T I[9]; + + /* dynamics */ + A[0] = 1.0F; + A[3] = dT; + A[6] = -0.5F * dT * dT; + A[1] = 0.0F; + A[4] = 1.0F; + A[7] = -dT; + for (i = 0; i < 3; i++) { + A[2 + 3 * i] = (real32_T)iv0[i]; + } + + /* prediction */ + K[0] = 0.5F * dT * dT; + K[1] = dT; + K[2] = 0.0F; + for (i = 0; i < 3; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + f0 += A[i + 3 * i0] * x_aposteriori_k[i0]; + } + + x_aposteriori[i] = f0 + K[i] * u; + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A[i + 3 * i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0]; + } + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + f0 = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1]; + } + + P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0]; + } + } + + if ((real32_T)fabs(u) < thresh) { + x_aposteriori[1] *= decay; + } + + /* update */ + if (gps_update == 1) { + f0 = 0.0F; + for (i = 0; i < 3; i++) { + f0 += (real32_T)iv1[i] * x_aposteriori[i]; + fv0[i] = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i]; + } + } + + y = z - f0; + f0 = 0.0F; + for (i = 0; i < 3; i++) { + f0 += fv0[i] * (real32_T)iv2[i]; + } + + S = f0 + (real32_T)R; + for (i = 0; i < 3; i++) { + f0 = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0]; + } + + K[i] = f0 / S; + } + + for (i = 0; i < 3; i++) { + x_aposteriori[i] += K[i] * y; + } + + for (i = 0; i < 9; i++) { + I[i] = 0; + } + + for (i = 0; i < 3; i++) { + I[i + 3 * i] = 1; + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + P_aposteriori[i + 3 * i0] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0]; + } + } + } + } else { + for (i = 0; i < 9; i++) { + P_aposteriori[i] = P_apriori[i]; + } + } +} + +/* End of code generation (positionKalmanFilter1D_dT.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h new file mode 100755 index 000000000..94cbe2ce6 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_dT.h + * + * Code generation for function 'positionKalmanFilter1D_dT' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_DT_H__ +#define __POSITIONKALMANFILTER1D_DT_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_dT_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]); +#endif +/* End of code generation (positionKalmanFilter1D_dT.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c new file mode 100755 index 000000000..aa89f8a9d --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_dT_initialize.c + * + * Code generation for function 'positionKalmanFilter1D_dT_initialize' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D_dT.h" +#include "positionKalmanFilter1D_dT_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D_dT_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h new file mode 100755 index 000000000..8d358a9a3 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_dT_initialize.h + * + * Code generation for function 'positionKalmanFilter1D_dT_initialize' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__ +#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_dT_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D_dT_initialize(void); +#endif +/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c new file mode 100755 index 000000000..20ed2edbb --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_dT_terminate.c + * + * Code generation for function 'positionKalmanFilter1D_dT_terminate' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D_dT.h" +#include "positionKalmanFilter1D_dT_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D_dT_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h new file mode 100755 index 000000000..5eb5807a0 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_dT_terminate.h + * + * Code generation for function 'positionKalmanFilter1D_dT_terminate' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__ +#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_dT_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D_dT_terminate(void); +#endif +/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h new file mode 100755 index 000000000..43e5f016c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h @@ -0,0 +1,16 @@ +/* + * positionKalmanFilter1D_dT_types.h + * + * Code generation for function 'positionKalmanFilter1D_dT' + * + * C source code generated on: Fri Nov 30 17:37:33 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__ +#define __POSITIONKALMANFILTER1D_DT_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (positionKalmanFilter1D_dT_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c new file mode 100755 index 000000000..5bd87c390 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_initialize.c + * + * Code generation for function 'positionKalmanFilter1D_initialize' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D.h" +#include "positionKalmanFilter1D_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (positionKalmanFilter1D_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h new file mode 100755 index 000000000..44bce472f --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_initialize.h + * + * Code generation for function 'positionKalmanFilter1D_initialize' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__ +#define __POSITIONKALMANFILTER1D_INITIALIZE_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D_initialize(void); +#endif +/* End of code generation (positionKalmanFilter1D_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c new file mode 100755 index 000000000..41e11936f --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_terminate.c + * + * Code generation for function 'positionKalmanFilter1D_terminate' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "positionKalmanFilter1D.h" +#include "positionKalmanFilter1D_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void positionKalmanFilter1D_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (positionKalmanFilter1D_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h new file mode 100755 index 000000000..e84ea01bc --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h @@ -0,0 +1,31 @@ +/* + * positionKalmanFilter1D_terminate.h + * + * Code generation for function 'positionKalmanFilter1D_terminate' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__ +#define __POSITIONKALMANFILTER1D_TERMINATE_H__ +/* Include files */ +#include +#include +#include + +#include "rtwtypes.h" +#include "positionKalmanFilter1D_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void positionKalmanFilter1D_terminate(void); +#endif +/* End of code generation (positionKalmanFilter1D_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h new file mode 100755 index 000000000..4b473f56f --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h @@ -0,0 +1,16 @@ +/* + * positionKalmanFilter1D_types.h + * + * Code generation for function 'positionKalmanFilter1D' + * + * C source code generated on: Fri Nov 30 14:26:11 2012 + * + */ + +#ifndef __POSITIONKALMANFILTER1D_TYPES_H__ +#define __POSITIONKALMANFILTER1D_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (positionKalmanFilter1D_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/randn.c b/src/modules/position_estimator_mc/codegen/randn.c new file mode 100755 index 000000000..51aef7b76 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/randn.c @@ -0,0 +1,524 @@ +/* + * randn.c + * + * Code generation for function 'randn' + * + * C source code generated on: Tue Feb 19 15:26:32 2013 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "kalman_dlqe3.h" +#include "randn.h" +#include "kalman_dlqe3_data.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ +static uint32_T d_state[625]; + +/* Function Declarations */ +static real_T b_genrandu(uint32_T mt[625]); +static real_T eml_rand_mt19937ar(uint32_T e_state[625]); +static real_T eml_rand_shr3cong(uint32_T e_state[2]); +static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]); +static void genrandu(uint32_T s, uint32_T *e_state, real_T *r); +static void twister_state_vector(uint32_T mt[625], real_T seed); + +/* Function Definitions */ +static real_T b_genrandu(uint32_T mt[625]) +{ + real_T r; + int32_T exitg1; + uint32_T u[2]; + boolean_T isvalid; + int32_T k; + boolean_T exitg2; + + /* This is a uniform (0,1) pseudorandom number generator based on: */ + /* */ + /* A C-program for MT19937, with initialization improved 2002/1/26. */ + /* Coded by Takuji Nishimura and Makoto Matsumoto. */ + /* */ + /* Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */ + /* All rights reserved. */ + /* */ + /* Redistribution and use in source and binary forms, with or without */ + /* modification, are permitted provided that the following conditions */ + /* are met: */ + /* */ + /* 1. Redistributions of source code must retain the above copyright */ + /* notice, this list of conditions and the following disclaimer. */ + /* */ + /* 2. Redistributions in binary form must reproduce the above copyright */ + /* notice, this list of conditions and the following disclaimer in the */ + /* documentation and/or other materials provided with the distribution. */ + /* */ + /* 3. The names of its contributors may not be used to endorse or promote */ + /* products derived from this software without specific prior written */ + /* permission. */ + /* */ + /* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */ + /* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */ + /* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */ + /* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR */ + /* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */ + /* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */ + /* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */ + /* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */ + /* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */ + /* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */ + /* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ + do { + exitg1 = 0; + genrand_uint32_vector(mt, u); + r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T) + (u[1] >> 6U)); + if (r == 0.0) { + if ((mt[624] >= 1U) && (mt[624] < 625U)) { + isvalid = TRUE; + } else { + isvalid = FALSE; + } + + if (isvalid) { + isvalid = FALSE; + k = 1; + exitg2 = FALSE; + while ((exitg2 == FALSE) && (k < 625)) { + if (mt[k - 1] == 0U) { + k++; + } else { + isvalid = TRUE; + exitg2 = TRUE; + } + } + } + + if (!isvalid) { + twister_state_vector(mt, 5489.0); + } + } else { + exitg1 = 1; + } + } while (exitg1 == 0); + + return r; +} + +static real_T eml_rand_mt19937ar(uint32_T e_state[625]) +{ + real_T r; + int32_T exitg1; + uint32_T u32[2]; + int32_T i; + static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068, + 0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787, + 0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557, + 0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991, + 0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419, + 0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975, + 0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107, + 0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997, + 0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153, + 0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165, + 0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959, + 0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086, + 0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062, + 0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548, + 1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093, + 1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034, + 1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357, + 1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815, + 1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083, + 1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944, + 1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653, + 1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949, + 1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382, + 1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073, + 1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936, + 1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328, + 1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998, + 1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547, + 1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872, + 1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808, + 1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049, + 1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761, + 1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417, + 1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591, + 1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609, + 1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487, + 1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818, + 1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719, + 1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782, + 1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172, + 1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452, + 1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082, + 1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281, + 1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127, + 1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043, + 1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569, + 1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366, + 1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053, + 2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753, + 2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983, + 2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952, + 2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431, + 2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416, + 2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379, + 2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137, + 2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729, + 2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715, + 2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105, + 2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184, + 2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674, + 2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999, + 2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341, + 2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214, + 3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143, + 3.65415288536101, 3.91075795952492 }; + + real_T u; + static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108, + 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131, + 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857, + 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884, + 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964, + 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187, + 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157, + 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863, + 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512, + 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774, + 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697, + 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624, + 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914, + 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987, + 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354, + 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766, + 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952, + 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322, + 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468, + 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458, + 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773, + 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716, + 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804, + 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022, + 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188, + 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832, + 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393, + 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032, + 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077, + 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522, + 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601, + 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337, + 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803, + 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183, + 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668, + 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261, + 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942, + 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501, + 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681, + 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331, + 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881, + 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135, + 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268, + 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839, + 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366, + 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263, + 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514, + 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669, + 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597, + 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839, + 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487, + 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009, + 0.093365311912691, 0.0911136480663738, 0.0888735920682759, + 0.0866451944505581, 0.0844285095703535, 0.082223595813203, + 0.0800305158146631, 0.0778493367020961, 0.0756801303589272, + 0.0735229737139814, 0.0713779490588905, 0.0692451443970068, + 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954, + 0.0587679529209339, 0.0567106901062031, 0.0546664613248891, + 0.0526354182767924, 0.0506177238609479, 0.0486135532158687, + 0.0466230949019305, 0.0446465522512946, 0.0426841449164746, + 0.0407361106559411, 0.0388027074045262, 0.0368842156885674, + 0.0349809414617162, 0.0330932194585786, 0.0312214171919203, + 0.0293659397581334, 0.0275272356696031, 0.0257058040085489, + 0.0239022033057959, 0.0221170627073089, 0.0203510962300445, + 0.0186051212757247, 0.0168800831525432, 0.0151770883079353, + 0.0134974506017399, 0.0118427578579079, 0.0102149714397015, + 0.00861658276939875, 0.00705087547137324, 0.00552240329925101, + 0.00403797259336304, 0.00260907274610216, 0.0012602859304986, + 0.000477467764609386 }; + + real_T x; + do { + exitg1 = 0; + genrand_uint32_vector(e_state, u32); + i = (int32_T)((u32[1] >> 24U) + 1U); + r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] & + 16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i]; + if (fabs(r) <= dv1[i - 1]) { + exitg1 = 1; + } else if (i < 256) { + u = b_genrandu(e_state); + if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) { + exitg1 = 1; + } + } else { + do { + u = b_genrandu(e_state); + x = log(u) * 0.273661237329758; + u = b_genrandu(e_state); + } while (!(-2.0 * log(u) > x * x)); + + if (r < 0.0) { + r = x - 3.65415288536101; + } else { + r = 3.65415288536101 - x; + } + + exitg1 = 1; + } + } while (exitg1 == 0); + + return r; +} + +static real_T eml_rand_shr3cong(uint32_T e_state[2]) +{ + real_T r; + uint32_T icng; + uint32_T jsr; + uint32_T ui; + int32_T j; + static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427, + 0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948, + 0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738, + 1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008, + 1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122, + 1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503, + 1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831, + 1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713, + 2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658, + 2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 }; + + real_T x; + real_T y; + real_T s; + icng = 69069U * e_state[0] + 1234567U; + jsr = e_state[1] ^ e_state[1] << 13U; + jsr ^= jsr >> 17U; + jsr ^= jsr << 5U; + ui = icng + jsr; + j = (int32_T)(ui & 63U); + r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1]; + if (fabs(r) <= dv0[j]) { + } else { + x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]); + icng = 69069U * icng + 1234567U; + jsr ^= jsr << 13U; + jsr ^= jsr >> 17U; + jsr ^= jsr << 5U; + y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10; + s = x + (0.5 + y); + if (s > 1.301198) { + if (r < 0.0) { + r = 0.4878992 * x - 0.4878992; + } else { + r = 0.4878992 - 0.4878992 * x; + } + } else if (s <= 0.9689279) { + } else { + s = 0.4878992 * x; + x = 0.4878992 - 0.4878992 * x; + if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) { + if (r < 0.0) { + r = -(0.4878992 - s); + } else { + r = 0.4878992 - s; + } + } else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 / + dv0[j + 1] <= exp(-0.5 * r * r)) { + } else { + do { + icng = 69069U * icng + 1234567U; + jsr ^= jsr << 13U; + jsr ^= jsr >> 17U; + jsr ^= jsr << 5U; + x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) / + 2.776994; + icng = 69069U * icng + 1234567U; + jsr ^= jsr << 13U; + jsr ^= jsr >> 17U; + jsr ^= jsr << 5U; + } while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) * + 2.328306436538696E-10) > x * x)); + + if (r < 0.0) { + r = x - 2.776994; + } else { + r = 2.776994 - x; + } + } + } + } + + e_state[0] = icng; + e_state[1] = jsr; + return r; +} + +static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]) +{ + int32_T i; + uint32_T mti; + int32_T kk; + uint32_T y; + uint32_T b_y; + uint32_T c_y; + uint32_T d_y; + for (i = 0; i < 2; i++) { + u[i] = 0U; + } + + for (i = 0; i < 2; i++) { + mti = mt[624] + 1U; + if (mti >= 625U) { + for (kk = 0; kk < 227; kk++) { + y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U); + if ((int32_T)(y & 1U) == 0) { + b_y = y >> 1U; + } else { + b_y = y >> 1U ^ 2567483615U; + } + + mt[kk] = mt[397 + kk] ^ b_y; + } + + for (kk = 0; kk < 396; kk++) { + y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U); + if ((int32_T)(y & 1U) == 0) { + c_y = y >> 1U; + } else { + c_y = y >> 1U ^ 2567483615U; + } + + mt[227 + kk] = mt[kk] ^ c_y; + } + + y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U); + if ((int32_T)(y & 1U) == 0) { + d_y = y >> 1U; + } else { + d_y = y >> 1U ^ 2567483615U; + } + + mt[623] = mt[396] ^ d_y; + mti = 1U; + } + + y = mt[(int32_T)mti - 1]; + mt[624] = mti; + y ^= y >> 11U; + y ^= y << 7U & 2636928640U; + y ^= y << 15U & 4022730752U; + y ^= y >> 18U; + u[i] = y; + } +} + +static void genrandu(uint32_T s, uint32_T *e_state, real_T *r) +{ + int32_T hi; + uint32_T test1; + uint32_T test2; + hi = (int32_T)(s / 127773U); + test1 = 16807U * (s - (uint32_T)hi * 127773U); + test2 = 2836U * (uint32_T)hi; + if (test1 < test2) { + *e_state = (test1 - test2) + 2147483647U; + } else { + *e_state = test1 - test2; + } + + *r = (real_T)*e_state * 4.6566128752457969E-10; +} + +static void twister_state_vector(uint32_T mt[625], real_T seed) +{ + uint32_T r; + int32_T mti; + if (seed < 4.294967296E+9) { + if (seed >= 0.0) { + r = (uint32_T)seed; + } else { + r = 0U; + } + } else if (seed >= 4.294967296E+9) { + r = MAX_uint32_T; + } else { + r = 0U; + } + + mt[0] = r; + for (mti = 0; mti < 623; mti++) { + r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti); + mt[1 + mti] = r; + } + + mt[624] = 624U; +} + +real_T randn(void) +{ + real_T r; + uint32_T e_state; + real_T t; + real_T b_r; + uint32_T f_state; + if (method == 0U) { + if (b_method == 4U) { + do { + genrandu(b_state, &e_state, &r); + genrandu(e_state, &b_state, &t); + b_r = 2.0 * r - 1.0; + t = 2.0 * t - 1.0; + t = t * t + b_r * b_r; + } while (!(t <= 1.0)); + + r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t); + } else if (b_method == 5U) { + r = eml_rand_shr3cong(c_state); + } else { + if (!state_not_empty) { + memset(&d_state[0], 0, 625U * sizeof(uint32_T)); + twister_state_vector(d_state, 5489.0); + state_not_empty = TRUE; + } + + r = eml_rand_mt19937ar(d_state); + } + } else if (method == 4U) { + e_state = state[0]; + do { + genrandu(e_state, &f_state, &r); + genrandu(f_state, &e_state, &t); + b_r = 2.0 * r - 1.0; + t = 2.0 * t - 1.0; + t = t * t + b_r * b_r; + } while (!(t <= 1.0)); + + state[0] = e_state; + r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t); + } else { + r = eml_rand_shr3cong(state); + } + + return r; +} + +/* End of code generation (randn.c) */ diff --git a/src/modules/position_estimator_mc/codegen/randn.h b/src/modules/position_estimator_mc/codegen/randn.h new file mode 100755 index 000000000..8a2aa9277 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/randn.h @@ -0,0 +1,33 @@ +/* + * randn.h + * + * Code generation for function 'randn' + * + * C source code generated on: Tue Feb 19 15:26:32 2013 + * + */ + +#ifndef __RANDN_H__ +#define __RANDN_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "kalman_dlqe3_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern real_T randn(void); +#endif +/* End of code generation (randn.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.c b/src/modules/position_estimator_mc/codegen/rtGetInf.c new file mode 100755 index 000000000..c6fa7884e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetInf.c @@ -0,0 +1,139 @@ +/* + * rtGetInf.c + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, Inf and MinusInf + */ +#include "rtGetInf.h" +#define NumBitsPerChar 8U + +/* Function: rtGetInf ================================================== + * Abstract: + * Initialize rtInf needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetInf(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T inf = 0.0; + if (bitsPerReal == 32U) { + inf = rtGetInfF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + } + } + + return inf; +} + +/* Function: rtGetInfF ================================================== + * Abstract: + * Initialize rtInfF needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetInfF(void) +{ + IEEESingle infF; + infF.wordL.wordLuint = 0x7F800000U; + return infF.wordL.wordLreal; +} + +/* Function: rtGetMinusInf ================================================== + * Abstract: + * Initialize rtMinusInf needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetMinusInf(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T minf = 0.0; + if (bitsPerReal == 32U) { + minf = rtGetMinusInfF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + } + } + + return minf; +} + +/* Function: rtGetMinusInfF ================================================== + * Abstract: + * Initialize rtMinusInfF needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetMinusInfF(void) +{ + IEEESingle minfF; + minfF.wordL.wordLuint = 0xFF800000U; + return minfF.wordL.wordLreal; +} + +/* End of code generation (rtGetInf.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.h b/src/modules/position_estimator_mc/codegen/rtGetInf.h new file mode 100755 index 000000000..e7b2a2d1c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetInf.h @@ -0,0 +1,23 @@ +/* + * rtGetInf.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +#ifndef __RTGETINF_H__ +#define __RTGETINF_H__ + +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetInf(void); +extern real32_T rtGetInfF(void); +extern real_T rtGetMinusInf(void); +extern real32_T rtGetMinusInfF(void); + +#endif +/* End of code generation (rtGetInf.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.c b/src/modules/position_estimator_mc/codegen/rtGetNaN.c new file mode 100755 index 000000000..552770149 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetNaN.c @@ -0,0 +1,96 @@ +/* + * rtGetNaN.c + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, NaN + */ +#include "rtGetNaN.h" +#define NumBitsPerChar 8U + +/* Function: rtGetNaN ================================================== + * Abstract: + * Initialize rtNaN needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetNaN(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T nan = 0.0; + if (bitsPerReal == 32U) { + nan = rtGetNaNF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF80000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + nan = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; + tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; + nan = tmpVal.fltVal; + break; + } + } + } + + return nan; +} + +/* Function: rtGetNaNF ================================================== + * Abstract: + * Initialize rtNaNF needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetNaNF(void) +{ + IEEESingle nanF = { { 0 } }; + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + nanF.wordL.wordLuint = 0xFFC00000U; + break; + } + + case BigEndian: + { + nanF.wordL.wordLuint = 0x7FFFFFFFU; + break; + } + } + + return nanF.wordL.wordLreal; +} + +/* End of code generation (rtGetNaN.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.h b/src/modules/position_estimator_mc/codegen/rtGetNaN.h new file mode 100755 index 000000000..5acdd9790 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetNaN.h @@ -0,0 +1,21 @@ +/* + * rtGetNaN.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +#ifndef __RTGETNAN_H__ +#define __RTGETNAN_H__ + +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetNaN(void); +extern real32_T rtGetNaNF(void); + +#endif +/* End of code generation (rtGetNaN.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c new file mode 100755 index 000000000..de121c4a0 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c @@ -0,0 +1,87 @@ +/* + * rt_nonfinite.c + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finites, + * (Inf, NaN and -Inf). + */ +#include "rt_nonfinite.h" +#include "rtGetNaN.h" +#include "rtGetInf.h" + +real_T rtInf; +real_T rtMinusInf; +real_T rtNaN; +real32_T rtInfF; +real32_T rtMinusInfF; +real32_T rtNaNF; + +/* Function: rt_InitInfAndNaN ================================================== + * Abstract: + * Initialize the rtInf, rtMinusInf, and rtNaN needed by the + * generated code. NaN is initialized as non-signaling. Assumes IEEE. + */ +void rt_InitInfAndNaN(size_t realSize) +{ + (void) (realSize); + rtNaN = rtGetNaN(); + rtNaNF = rtGetNaNF(); + rtInf = rtGetInf(); + rtInfF = rtGetInfF(); + rtMinusInf = rtGetMinusInf(); + rtMinusInfF = rtGetMinusInfF(); +} + +/* Function: rtIsInf ================================================== + * Abstract: + * Test if value is infinite + */ +boolean_T rtIsInf(real_T value) +{ + return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); +} + +/* Function: rtIsInfF ================================================= + * Abstract: + * Test if single-precision value is infinite + */ +boolean_T rtIsInfF(real32_T value) +{ + return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); +} + +/* Function: rtIsNaN ================================================== + * Abstract: + * Test if value is not a number + */ +boolean_T rtIsNaN(real_T value) +{ +#if defined(_MSC_VER) && (_MSC_VER <= 1200) + return _isnan(value)? TRUE:FALSE; +#else + return (value!=value)? 1U:0U; +#endif +} + +/* Function: rtIsNaNF ================================================= + * Abstract: + * Test if single-precision value is not a number + */ +boolean_T rtIsNaNF(real32_T value) +{ +#if defined(_MSC_VER) && (_MSC_VER <= 1200) + return _isnan((real_T)value)? true:false; +#else + return (value!=value)? 1U:0U; +#endif +} + + +/* End of code generation (rt_nonfinite.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h new file mode 100755 index 000000000..3bbcfd087 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h @@ -0,0 +1,53 @@ +/* + * rt_nonfinite.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +#ifndef __RT_NONFINITE_H__ +#define __RT_NONFINITE_H__ + +#if defined(_MSC_VER) && (_MSC_VER <= 1200) +#include +#endif +#include +#include "rtwtypes.h" + +extern real_T rtInf; +extern real_T rtMinusInf; +extern real_T rtNaN; +extern real32_T rtInfF; +extern real32_T rtMinusInfF; +extern real32_T rtNaNF; +extern void rt_InitInfAndNaN(size_t realSize); +extern boolean_T rtIsInf(real_T value); +extern boolean_T rtIsInfF(real32_T value); +extern boolean_T rtIsNaN(real_T value); +extern boolean_T rtIsNaNF(real32_T value); + +typedef struct { + struct { + uint32_T wordH; + uint32_T wordL; + } words; +} BigEndianIEEEDouble; + +typedef struct { + struct { + uint32_T wordL; + uint32_T wordH; + } words; +} LittleEndianIEEEDouble; + +typedef struct { + union { + real32_T wordLreal; + uint32_T wordLuint; + } wordL; +} IEEESingle; + +#endif +/* End of code generation (rt_nonfinite.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rtwtypes.h b/src/modules/position_estimator_mc/codegen/rtwtypes.h new file mode 100755 index 000000000..8916e8572 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtwtypes.h @@ -0,0 +1,159 @@ +/* + * rtwtypes.h + * + * Code generation for function 'kalman_dlqe2' + * + * C source code generated on: Thu Feb 14 12:52:29 2013 + * + */ + +#ifndef __RTWTYPES_H__ +#define __RTWTYPES_H__ +#ifndef TRUE +# define TRUE (1U) +#endif +#ifndef FALSE +# define FALSE (0U) +#endif +#ifndef __TMWTYPES__ +#define __TMWTYPES__ + +#include + +/*=======================================================================* + * Target hardware information + * Device type: Generic->MATLAB Host Computer + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Undefined + * Shift right on a signed integer as arithmetic shift: off + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * + * ulong_T, char_T and byte_T. * + *===========================================================================*/ + +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef char_T byte_T; + +/*===========================================================================* + * Complex number type definitions * + *===========================================================================*/ +#define CREAL_T + typedef struct { + real32_T re; + real32_T im; + } creal32_T; + + typedef struct { + real64_T re; + real64_T im; + } creal64_T; + + typedef struct { + real_T re; + real_T im; + } creal_T; + + typedef struct { + int8_T re; + int8_T im; + } cint8_T; + + typedef struct { + uint8_T re; + uint8_T im; + } cuint8_T; + + typedef struct { + int16_T re; + int16_T im; + } cint16_T; + + typedef struct { + uint16_T re; + uint16_T im; + } cuint16_T; + + typedef struct { + int32_T re; + int32_T im; + } cint32_T; + + typedef struct { + uint32_T re; + uint32_T im; + } cuint32_T; + + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255)) +#define MIN_uint8_T ((uint8_T)(0)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535)) +#define MIN_uint16_T ((uint16_T)(0)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MIN_uint32_T ((uint32_T)(0)) + +/* Logical type definitions */ +#if !defined(__cplusplus) && !defined(__true_false_are_keywords) +# ifndef false +# define false (0U) +# endif +# ifndef true +# define true (1U) +# endif +#endif + +/* + * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation + * for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable) + * including the null-termination character. Referenced by + * rt_logging.c and rt_matrx.c. + */ +#define TMW_NAME_LENGTH_MAX 64 + +#endif +#endif +/* End of code generation (rtwtypes.h) */ -- cgit v1.2.3