diff options
author | px4dev <px4@purgatory.org> | 2013-04-26 12:35:45 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-04-26 12:35:45 -0700 |
commit | 187f7603b9e3ccbf5ac858d250931d583b52d9eb (patch) | |
tree | 059af67adc5fc7edbcf2b15cdd28cf07e7cf3eb6 /apps/position_estimator_mc/codegen/kalman_dlqe1.c | |
parent | 9169ceb3f4884a863d527c6b8e7ea237b41a48ce (diff) | |
parent | 3acdc9d4ce3d83af6bb7f953e466620be690658e (diff) | |
download | px4-firmware-187f7603b9e3ccbf5ac858d250931d583b52d9eb.tar.gz px4-firmware-187f7603b9e3ccbf5ac858d250931d583b52d9eb.tar.bz2 px4-firmware-187f7603b9e3ccbf5ac858d250931d583b52d9eb.zip |
Merge branch 'export-build' into fmuv2_bringup
Sync with master via export-build.
Diffstat (limited to 'apps/position_estimator_mc/codegen/kalman_dlqe1.c')
-rwxr-xr-x | apps/position_estimator_mc/codegen/kalman_dlqe1.c | 58 |
1 files changed, 58 insertions, 0 deletions
diff --git a/apps/position_estimator_mc/codegen/kalman_dlqe1.c b/apps/position_estimator_mc/codegen/kalman_dlqe1.c new file mode 100755 index 000000000..977565b8e --- /dev/null +++ b/apps/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) */ |