From 9de6a815434e15ee7add1ee138ac69a87733287b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 2 Oct 2012 09:23:47 +0200 Subject: [PATCH] Updated EKF filter, fixed uploader (reverted to master) --- Tools/px_uploader.py | 17 +- apps/attitude_estimator_ekf/Makefile | 4 +- .../attitude_estimator_ekf_main.c | 135 +- .../codegen/attitudeKalmanfilter.c | 1148 +++++++++-------- .../codegen/attitudeKalmanfilter.h | 4 +- .../codegen/attitudeKalmanfilter_initialize.c | 2 +- .../codegen/attitudeKalmanfilter_initialize.h | 2 +- .../codegen/attitudeKalmanfilter_terminate.c | 2 +- .../codegen/attitudeKalmanfilter_terminate.h | 2 +- .../codegen/attitudeKalmanfilter_types.h | 2 +- apps/attitude_estimator_ekf/codegen/cross.c | 2 +- apps/attitude_estimator_ekf/codegen/cross.h | 2 +- apps/attitude_estimator_ekf/codegen/diag.c | 40 +- apps/attitude_estimator_ekf/codegen/diag.h | 7 +- apps/attitude_estimator_ekf/codegen/eye.c | 10 +- apps/attitude_estimator_ekf/codegen/eye.h | 4 +- apps/attitude_estimator_ekf/codegen/find.c | 77 -- .../attitude_estimator_ekf/codegen/mrdivide.c | 902 ++++--------- .../attitude_estimator_ekf/codegen/mrdivide.h | 6 +- apps/attitude_estimator_ekf/codegen/norm.c | 2 +- apps/attitude_estimator_ekf/codegen/norm.h | 2 +- apps/attitude_estimator_ekf/codegen/power.c | 84 ++ .../codegen/{find.h => power.h} | 14 +- .../attitude_estimator_ekf/codegen/rtGetInf.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetInf.h | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.h | 2 +- .../codegen/rt_defines.h | 2 +- .../codegen/rt_nonfinite.c | 2 +- .../codegen/rt_nonfinite.h | 2 +- .../attitude_estimator_ekf/codegen/rtwtypes.h | 2 +- 31 files changed, 1112 insertions(+), 1374 deletions(-) delete mode 100755 apps/attitude_estimator_ekf/codegen/find.c create mode 100755 apps/attitude_estimator_ekf/codegen/power.c rename apps/attitude_estimator_ekf/codegen/{find.h => power.h} (55%) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 2b5a6edde9..7ebd37e750 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -263,8 +263,8 @@ class uploader(object): print("program...") self.__program(fw) - #print("verify...") - #self.__verify(fw) + print("verify...") + self.__verify(fw) print("done, rebooting.") self.__reboot() @@ -290,7 +290,18 @@ while True: # create an uploader attached to the port try: - up = uploader("\\\\.\\COM2", args.baud) + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if not "COM" in port and not "tty.usb" in port: + up = uploader(port, args.baud) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if not "COM" in port and not "ACM" in port: + up = uploader(port, args.baud) + elif "win" in _platform: + # Windows, don't open POSIX ports + if not "/" in port: + up = uploader(port, args.baud) except: # open failed, rate-limit our attempts time.sleep(0.05) diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index 932b49f5c8..a8f80fd4c7 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -45,9 +45,9 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rtGetInf.c \ codegen/rtGetNaN.c \ codegen/norm.c \ - codegen/find.c \ codegen/diag.c \ - codegen/cross.c + codegen/cross.c \ + codegen/power.c # XXX this is *horribly* broken INCLUDES += $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index e443f6295d..c1f8380e6a 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -75,38 +75,45 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds -static float dt = 1; +static float dt = 1.0f; /* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ -static float z_k[9] = {0}; /**< Measurement vector */ -static float x_aposteriori_k[9] = {0,0,0,0,0,-9.81,1,1,-1}; /**< */ -static float x_aposteriori[9] = {0}; -static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, +static float z_k[9]; /**< Measurement vector */ +static float x_aposteriori_k[12]; /**< */ +static float x_aposteriori[12]; +static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, }; -static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, +static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, }; /**< init: diagonal matrix with big values */ -static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ +// static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ static float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f }; /**< init: identity matrix */ + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ @@ -229,6 +236,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + + /* process noise covariance */ + float q[12]; + /* measurement noise covariance */ + float r[9]; + /* output euler angles */ + float euler[3] = {0.0f, 0.0f, 0.0f}; + /* Main loop*/ while (!thread_should_exit) { @@ -278,10 +293,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) overloadcounter++; } - int8_t update_vect[3] = {1, 1, 1}; - float euler[3]; + uint8_t update_vect[3] = {1, 1, 1}; int32_t z_k_sizes = 9; - float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; static bool const_initialized = false; @@ -289,38 +303,41 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) { dt = 0.005f; - q[0] = 1e1; - q[1] = 1e1; - q[2] = 1e1; - q[3] = 1e-6; - q[4] = 1e-6; - q[5] = 1e-6; - q[6] = 1e-1; - q[7] = 1e-1; - q[8] = 1e-1; - q[9] = 1e-1; - q[10] = 1e-1; - q[11] = 1e-1; + q[0] = 1e1f; + q[1] = 1e1f; + q[2] = 1e1f; + q[3] = 1e-6f; + q[4] = 1e-6f; + q[5] = 1e-6f; + q[6] = 1e-1f; + q[7] = 1e-1f; + q[8] = 1e-1f; + q[9] = 1e-1f; + q[10] = 1e-1f; + q[11] = 1e-1f; - r[0]= 1e-2; - r[1]= 1e-2; - r[2]= 1e-2; - r[3]= 1e-1; - r[4]= 1e-1; - r[5]= 1e-1; - r[6]= 1e-1; - r[7]= 1e-1; - r[8]= 1e-1; + r[0]= 1e-2f; + r[1]= 1e-2f; + r[2]= 1e-2f; + r[3]= 1e-1f; + r[4]= 1e-1f; + r[5]= 1e-1f; + r[6]= 1e-1f; + r[7]= 1e-1f; + r[8]= 1e-1f; x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; x_aposteriori_k[2] = z_k[2]; - x_aposteriori_k[3] = z_k[3]; - x_aposteriori_k[4] = z_k[4]; - x_aposteriori_k[5] = z_k[5]; - x_aposteriori_k[6] = z_k[6]; - x_aposteriori_k[7] = z_k[7]; - x_aposteriori_k[8] = z_k[8]; + x_aposteriori_k[3] = 0.0f; + x_aposteriori_k[4] = 0.0f; + x_aposteriori_k[5] = 0.0f; + x_aposteriori_k[6] = z_k[3]; + x_aposteriori_k[7] = z_k[4]; + x_aposteriori_k[8] = z_k[5]; + x_aposteriori_k[9] = z_k[6]; + x_aposteriori_k[10] = z_k[7]; + x_aposteriori_k[11] = z_k[8]; const_initialized = true; } @@ -331,15 +348,17 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } uint64_t timing_start = hrt_absolute_time(); - attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, - Rot_matrix, x_aposteriori, P_aposteriori); + // attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, + // Rot_matrix, x_aposteriori, P_aposteriori); + attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, q, r, + euler, Rot_matrix, x_aposteriori, P_aposteriori); /* swap values for next iteration */ memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); uint64_t timing_diff = hrt_absolute_time() - timing_start; /* print rotation matrix every 200th time */ - if (printcounter % 2 == 0) { + if (printcounter % 200 == 0) { // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], @@ -348,8 +367,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) // } - // printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); - // printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", (double)euler[0], (double)euler[1], (double)euler[2]); + printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 459dcc9b9d..86d872fd2c 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:42 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ @@ -15,7 +15,7 @@ #include "eye.h" #include "mrdivide.h" #include "diag.h" -#include "find.h" +#include "power.h" /* Type Definitions */ @@ -52,662 +52,696 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) } /* - * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,updVect,z_k,u,x_aposteriori_k,P_aposteriori_k,knownConst) + * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r) */ -void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T - z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T - x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T - knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T - x_aposteriori[9], real32_T P_aposteriori[81]) +void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const + real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T + P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T + eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T + P_aposteriori[144]) { - int32_T udpIndVect_sizes; - real_T udpIndVect_data[9]; - real32_T R_temp[9]; + real32_T a[12]; + int32_T i; + real32_T b_a[12]; + real32_T Q[144]; + real32_T O[9]; real_T dv0[9]; - int32_T ib; - int32_T i0; - real32_T fv0[81]; - real32_T b_knownConst[27]; - real32_T fv1[9]; - real32_T c_knownConst[9]; - real_T dv1[9]; - real_T dv2[9]; - real32_T A_lin[81]; + real32_T c_a[9]; + real32_T d_a[9]; real32_T x_n_b[3]; - real32_T K_k_data[81]; + real32_T z_n_b[3]; + real32_T x_apriori[12]; + real32_T y_n_b[3]; + int32_T i0; + real32_T e_a[3]; + real_T dv1[144]; + real32_T A_lin[144]; + real32_T b_A_lin[144]; int32_T i1; - real32_T b_A_lin[81]; - static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + real32_T y; + real32_T P_apriori[144]; + real32_T R[81]; + real32_T b_P_apriori[108]; + static const int8_T iv0[108] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - real32_T P_apriori[81]; - int32_T ia; - static const int8_T H_k_full[81] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + real32_T K_k[108]; + static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - int8_T H_k_data[81]; - int32_T accUpt; - int32_T magUpt; - real32_T y; - real32_T y_k_data[9]; - int32_T P_apriori_sizes[2]; - int32_T H_k_sizes[2]; - int32_T K_k_sizes[2]; - real32_T b_y[9]; - real_T dv3[81]; - real32_T c_y; - real32_T z_n_b[3]; - real32_T y_n_b[3]; + real32_T fv0[81]; + real32_T c_P_apriori[36]; + static const int8_T iv2[36] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0 }; - /* Extended Attitude Kalmanfilter */ + real32_T fv1[36]; + static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + real32_T S_k[36]; + real32_T d_P_apriori[72]; + static const int8_T iv4[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0 }; + + real32_T b_K_k[72]; + static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0 }; + + real32_T b_r[6]; + static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1 }; + + static const int8_T iv7[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1 }; + + real32_T fv2[6]; + real32_T b_z[6]; + real32_T b_y; + + /* Extended Attitude Kalmanfilter */ /* */ - /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ - /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ - /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ + /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ + /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ + /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ /* */ - /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ + /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ /* */ - /* Example.... */ + /* Example.... */ /* */ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ - /* %define the matrices */ - /* tpred=0.005; */ - /* */ - /* A=[ 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */ - /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */ - /* -1, 1, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */ - /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */ - /* -1, 1, 0, 0, 0, 0, 0, 0, 0]; */ - /* */ - /* */ - /* b=[70, 0, 0; */ - /* 0, 70, 0; */ - /* 0, 0, 0; */ - /* 0, 0, 0; */ - /* 0, 0, 0; */ - /* 0, 0, 0; */ - /* 0, 0, 0; */ - /* 0, 0, 0; */ - /* 0, 0, 0]; */ - /* */ - /* */ - /* C=[1, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, 1, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, 0, 1, 0, 0, 0, 0, 0, 0; */ - /* 0, 0, 0, 1, 0, 0, 0, 0, 0; */ - /* 0, 0, 0, 0, 1, 0, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 1, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 1, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 1, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 1]; */ - /* D=[]; */ - /* */ - /* */ - /* sys=ss(A,b,C,D); */ - /* */ - /* sysd=c2d(sys,tpred); */ - /* */ - /* */ - /* F=sysd.a; */ - /* */ - /* B=sysd.b; */ - /* */ - /* H=sysd.c; */ - /* 'attitudeKalmanfilter:68' udpIndVect=find(updVect); */ - find(updVect, udpIndVect_data, &udpIndVect_sizes); + /* coder.varsize('udpIndVect', [9,1], [1,0]) */ + /* udpIndVect=find(updVect); */ + /* process and measurement noise covariance matrix */ + /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */ + power(q, 2.0, a); + for (i = 0; i < 12; i++) { + b_a[i] = a[i] * dt; + } + + diag(b_a, Q); - /* 'attitudeKalmanfilter:71' rates_ProcessNoiseMatrix=diag([knownConst(1),knownConst(1),knownConst(2)]); */ - /* 'attitudeKalmanfilter:72' acc_ProcessNoise=knownConst(3); */ - /* 'attitudeKalmanfilter:73' mag_ProcessNoise=knownConst(4); */ - /* 'attitudeKalmanfilter:74' rates_MeasurementNoise=knownConst(6); */ - /* 'attitudeKalmanfilter:75' acc_MeasurementNoise=knownConst(7); */ - /* 'attitudeKalmanfilter:76' mag_MeasurementNoise=knownConst(8); */ - /* process noise covariance matrix */ - /* 'attitudeKalmanfilter:81' Q = [ rates_ProcessNoiseMatrix, zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:82' zeros(3), eye(3)*mag_ProcessNoise, zeros(3); */ - /* 'attitudeKalmanfilter:83' zeros(3), zeros(3), eye(3)*acc_ProcessNoise]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:89' H_k_full=[ eye(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:90' zeros(3), eye(3), zeros(3); */ - /* 'attitudeKalmanfilter:91' zeros(3), zeros(3), eye(3)]; */ - /* compute A(t,w) */ - /* x_aposteriori_k[10,11,12] should be [p,q,r] */ - /* R_temp=[1,-r, q */ - /* r, 1, -p */ - /* -q, p, 1] */ - /* 'attitudeKalmanfilter:100' R_temp=[1, -dt*x_aposteriori_k(3), dt*x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:101' dt*x_aposteriori_k(3), 1, -dt*x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:102' -dt*x_aposteriori_k(2), dt*x_aposteriori_k(1), 1]; */ - R_temp[0] = 1.0F; - R_temp[3] = -dt * x_aposteriori_k[2]; - R_temp[6] = dt * x_aposteriori_k[1]; - R_temp[1] = dt * x_aposteriori_k[2]; - R_temp[4] = 1.0F; - R_temp[7] = -dt * x_aposteriori_k[0]; - R_temp[2] = -dt * x_aposteriori_k[1]; - R_temp[5] = dt * x_aposteriori_k[0]; - R_temp[8] = 1.0F; + /* 'attitudeKalmanfilter:37' wx= x_aposteriori_k(1); */ + /* 'attitudeKalmanfilter:38' wy= x_aposteriori_k(2); */ + /* 'attitudeKalmanfilter:39' wz= x_aposteriori_k(3); */ + /* 'attitudeKalmanfilter:41' wox= x_aposteriori_k(4); */ + /* 'attitudeKalmanfilter:42' woy= x_aposteriori_k(5); */ + /* 'attitudeKalmanfilter:43' woz= x_aposteriori_k(6); */ + /* 'attitudeKalmanfilter:45' zex= x_aposteriori_k(7); */ + /* 'attitudeKalmanfilter:46' zey= x_aposteriori_k(8); */ + /* 'attitudeKalmanfilter:47' zez= x_aposteriori_k(9); */ + /* 'attitudeKalmanfilter:49' mux= x_aposteriori_k(10); */ + /* 'attitudeKalmanfilter:50' muy= x_aposteriori_k(11); */ + /* 'attitudeKalmanfilter:51' muz= x_aposteriori_k(12); */ + /* 'attitudeKalmanfilter:54' wk =[wx; */ + /* 'attitudeKalmanfilter:55' wy; */ + /* 'attitudeKalmanfilter:56' wz]; */ + /* 'attitudeKalmanfilter:58' wok =[wox;woy;woz]; */ + /* 'attitudeKalmanfilter:59' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + O[0] = 0.0F; + O[1] = -x_aposteriori_k[2]; + O[2] = x_aposteriori_k[1]; + O[3] = x_aposteriori_k[2]; + O[4] = 0.0F; + O[5] = -x_aposteriori_k[0]; + O[6] = -x_aposteriori_k[1]; + O[7] = x_aposteriori_k[0]; + O[8] = 0.0F; - /* 'attitudeKalmanfilter:106' A_pred=[eye(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:107' zeros(3), R_temp', zeros(3); */ - /* 'attitudeKalmanfilter:108' zeros(3), zeros(3), R_temp']; */ - /* 'attitudeKalmanfilter:110' B_pred=[knownConst(9)*dt, 0, 0; */ - /* 'attitudeKalmanfilter:111' 0, knownConst(10)*dt, 0; */ - /* 'attitudeKalmanfilter:112' 0, 0, 0; */ - /* 'attitudeKalmanfilter:113' 0, 0, 0; */ - /* 'attitudeKalmanfilter:114' 0, 0, 0; */ - /* 'attitudeKalmanfilter:115' 0, 0, 0; */ - /* 'attitudeKalmanfilter:116' 0, 0, 0; */ - /* 'attitudeKalmanfilter:117' 0, 0, 0; */ - /* 'attitudeKalmanfilter:118' 0, 0, 0]; */ - /* %prediction step */ - /* 'attitudeKalmanfilter:121' x_apriori=A_pred*x_aposteriori_k+B_pred*u(1:3); */ + /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ eye(dv0); - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib]; - } + for (i = 0; i < 9; i++) { + c_a[i] = (real32_T)dv0[i] + O[i] * dt; } - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[i0 + 9 * (ib + 3)] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[i0 + 9 * (ib + 6)] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * ib) + 3] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * (ib + 3)) + 3] = R_temp[ib + 3 * i0]; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * ib) + 6] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F; - } - } - - b_knownConst[0] = knownConst[8] * dt; - b_knownConst[9] = 0.0F; - b_knownConst[18] = 0.0F; - b_knownConst[1] = 0.0F; - b_knownConst[10] = knownConst[9] * dt; - b_knownConst[19] = 0.0F; - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * (ib + 6)) + 6] = R_temp[ib + 3 * i0]; - } - - b_knownConst[2 + 9 * ib] = 0.0F; - b_knownConst[3 + 9 * ib] = 0.0F; - b_knownConst[4 + 9 * ib] = 0.0F; - b_knownConst[5 + 9 * ib] = 0.0F; - b_knownConst[6 + 9 * ib] = 0.0F; - b_knownConst[7 + 9 * ib] = 0.0F; - b_knownConst[8 + 9 * ib] = 0.0F; - } - - for (ib = 0; ib < 9; ib++) { - fv1[ib] = 0.0F; - for (i0 = 0; i0 < 9; i0++) { - fv1[ib] += fv0[ib + 9 * i0] * x_aposteriori_k[i0]; - } - - c_knownConst[ib] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - c_knownConst[ib] += b_knownConst[ib + 9 * i0] * u[i0]; - } - - x_aposteriori[ib] = fv1[ib] + c_knownConst[ib]; - } - - /* linearization */ - /* 'attitudeKalmanfilter:125' temp_mat=[ 0, -dt, dt; */ - /* 'attitudeKalmanfilter:126' dt, 0, -dt; */ - /* 'attitudeKalmanfilter:127' -dt, dt, 0]; */ - R_temp[0] = 0.0F; - R_temp[3] = -dt; - R_temp[6] = dt; - R_temp[1] = dt; - R_temp[4] = 0.0F; - R_temp[7] = -dt; - R_temp[2] = -dt; - R_temp[5] = dt; - R_temp[8] = 0.0F; - - /* 'attitudeKalmanfilter:129' A_lin=[ eye(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:130' temp_mat, eye(3) , zeros(3); */ - /* 'attitudeKalmanfilter:131' temp_mat, zeros(3), eye(3)]; */ + /* 'attitudeKalmanfilter:61' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ eye(dv0); - eye(dv1); - eye(dv2); - for (ib = 0; ib < 3; ib++) { + for (i = 0; i < 9; i++) { + d_a[i] = (real32_T)dv0[i] + O[i] * dt; + } + + /* 'attitudeKalmanfilter:63' EZ=[0,zez,-zey; */ + /* 'attitudeKalmanfilter:64' -zez,0,zex; */ + /* 'attitudeKalmanfilter:65' zey,-zex,0]'; */ + /* 'attitudeKalmanfilter:66' MA=[0,muz,-muy; */ + /* 'attitudeKalmanfilter:67' -muz,0,mux; */ + /* 'attitudeKalmanfilter:68' zey,-mux,0]'; */ + /* 'attitudeKalmanfilter:72' E=eye(3); */ + /* 'attitudeKalmanfilter:73' Z=zeros(3); */ + /* 'attitudeKalmanfilter:74' x_apriori=[wk;wok;zek;muk]; */ + x_n_b[0] = x_aposteriori_k[6]; + x_n_b[1] = x_aposteriori_k[7]; + x_n_b[2] = x_aposteriori_k[8]; + z_n_b[0] = x_aposteriori_k[9]; + z_n_b[1] = x_aposteriori_k[10]; + z_n_b[2] = x_aposteriori_k[11]; + x_apriori[0] = x_aposteriori_k[0]; + x_apriori[1] = x_aposteriori_k[1]; + x_apriori[2] = x_aposteriori_k[2]; + x_apriori[3] = x_aposteriori_k[3]; + x_apriori[4] = x_aposteriori_k[4]; + x_apriori[5] = x_aposteriori_k[5]; + for (i = 0; i < 3; i++) { + y_n_b[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - A_lin[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib]; + y_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0]; + } + + e_a[i] = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + e_a[i] += d_a[i + 3 * i0] * z_n_b[i0]; + } + + x_apriori[i + 6] = y_n_b[i]; + } + + for (i = 0; i < 3; i++) { + x_apriori[i + 9] = e_a[i]; + } + + /* 'attitudeKalmanfilter:76' A_lin=[ Z, Z, Z, Z */ + /* 'attitudeKalmanfilter:77' Z, Z, Z, Z */ + /* 'attitudeKalmanfilter:78' EZ, Z, O, Z */ + /* 'attitudeKalmanfilter:79' MA, Z, Z, O]; */ + /* 'attitudeKalmanfilter:82' A_lin=eye(12)+A_lin*dt; */ + b_eye(dv1); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_lin[i0 + 12 * i] = 0.0F; + } + + for (i0 = 0; i0 < 3; i0++) { + A_lin[(i0 + 12 * i) + 3] = 0.0F; } } - for (ib = 0; ib < 3; ib++) { + A_lin[6] = 0.0F; + A_lin[7] = x_aposteriori_k[8]; + A_lin[8] = -x_aposteriori_k[7]; + A_lin[18] = -x_aposteriori_k[8]; + A_lin[19] = 0.0F; + A_lin[20] = x_aposteriori_k[6]; + A_lin[30] = x_aposteriori_k[7]; + A_lin[31] = -x_aposteriori_k[6]; + A_lin[32] = 0.0F; + for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { - A_lin[i0 + 9 * (ib + 3)] = 0.0F; + A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F; } } - for (ib = 0; ib < 3; ib++) { + for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { - A_lin[i0 + 9 * (ib + 6)] = 0.0F; + A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i]; } } - for (ib = 0; ib < 3; ib++) { + for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 9 * ib) + 3] = R_temp[i0 + 3 * ib]; + A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F; } } - for (ib = 0; ib < 3; ib++) { + A_lin[9] = 0.0F; + A_lin[10] = x_aposteriori_k[11]; + A_lin[11] = -x_aposteriori_k[10]; + A_lin[21] = -x_aposteriori_k[11]; + A_lin[22] = 0.0F; + A_lin[23] = x_aposteriori_k[9]; + A_lin[33] = x_aposteriori_k[7]; + A_lin[34] = -x_aposteriori_k[9]; + A_lin[35] = 0.0F; + for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 9 * (ib + 3)) + 3] = (real32_T)dv1[i0 + 3 * ib]; + A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F; } } - for (ib = 0; ib < 3; ib++) { + for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 9 * (ib + 6)) + 3] = 0.0F; + A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F; } } - for (ib = 0; ib < 3; ib++) { + for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 9 * ib) + 6] = R_temp[i0 + 3 * ib]; + A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i]; } } - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 9 * (ib + 3)) + 6] = 0.0F; + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] * + dt; } } - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 9 * (ib + 6)) + 6] = (real32_T)dv2[i0 + 3 * ib]; - } - } - - /* 'attitudeKalmanfilter:134' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ - x_n_b[0] = knownConst[0]; - x_n_b[1] = knownConst[0]; - x_n_b[2] = knownConst[1]; - diag(x_n_b, R_temp); - for (ib = 0; ib < 9; ib++) { - for (i0 = 0; i0 < 9; i0++) { - K_k_data[ib + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - K_k_data[ib + 9 * i0] += A_lin[ib + 9 * i1] * P_aposteriori_k[i1 + 9 * + /* 'attitudeKalmanfilter:88' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + A_lin[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * i0]; } } + } - for (i0 = 0; i0 < 9; i0++) { - b_A_lin[ib + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - b_A_lin[ib + 9 * i0] += K_k_data[ib + 9 * i1] * A_lin[i0 + 9 * i1]; + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + y += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; } + + P_apriori[i + 12 * i0] = y + Q[i + 12 * i0]; } } - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - K_k_data[i0 + 9 * ib] = R_temp[i0 + 3 * ib]; - } - } + /* %update */ + /* 'attitudeKalmanfilter:92' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ + if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { + /* 'attitudeKalmanfilter:93' R=diag(r); */ + b_diag(r, R); - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - K_k_data[i0 + 9 * (ib + 3)] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - K_k_data[i0 + 9 * (ib + 6)] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - K_k_data[(i0 + 9 * ib) + 3] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - K_k_data[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * - knownConst[3]; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - K_k_data[(i0 + 9 * (ib + 6)) + 3] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - K_k_data[(i0 + 9 * ib) + 6] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - K_k_data[(i0 + 9 * (ib + 3)) + 6] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - K_k_data[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * - knownConst[2]; - } - } - - for (ib = 0; ib < 9; ib++) { - for (i0 = 0; i0 < 9; i0++) { - P_apriori[i0 + 9 * ib] = b_A_lin[i0 + 9 * ib] + K_k_data[i0 + 9 * ib]; - } - } - - /* 'attitudeKalmanfilter:137' if ~isempty(udpIndVect)==1 */ - if (!(udpIndVect_sizes == 0) == 1) { - /* 'attitudeKalmanfilter:138' H_k= H_k_full(udpIndVect,:); */ - for (ib = 0; ib < 9; ib++) { - ia = udpIndVect_sizes - 1; - for (i0 = 0; i0 <= ia; i0++) { - H_k_data[i0 + udpIndVect_sizes * ib] = H_k_full[((int32_T) - udpIndVect_data[i0] + 9 * ib) - 1]; + /* observation matrix */ + /* 'attitudeKalmanfilter:96' H_k=[ E, E, Z, Z; */ + /* 'attitudeKalmanfilter:97' Z, Z, E, Z; */ + /* 'attitudeKalmanfilter:98' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:100' y_k=z(1:9)-H_k*x_apriori; */ + /* 'attitudeKalmanfilter:102' S_k=H_k*P_apriori*H_k'+R; */ + /* 'attitudeKalmanfilter:103' K_k=(P_apriori*H_k'/(S_k)); */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 9; i0++) { + b_P_apriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv0[i1 + + 12 * i0]; + } } } - /* %update step */ - /* 'attitudeKalmanfilter:140' accUpt=1; */ - accUpt = 1; + for (i = 0; i < 9; i++) { + for (i0 = 0; i0 < 12; i0++) { + K_k[i + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + K_k[i + 9 * i0] += (real32_T)iv1[i + 9 * i1] * P_apriori[i1 + 12 * i0]; + } + } + } - /* 'attitudeKalmanfilter:141' magUpt=1; */ - magUpt = 1; + for (i = 0; i < 9; i++) { + for (i0 = 0; i0 < 9; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + y += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0]; + } - /* 'attitudeKalmanfilter:142' y_k=z_k-H_k*x_apriori; */ - ia = udpIndVect_sizes - 1; - for (ib = 0; ib <= ia; ib++) { + fv0[i + 9 * i0] = y + R[i + 9 * i0]; + } + } + + mrdivide(b_P_apriori, fv0, K_k); + + /* 'attitudeKalmanfilter:106' x_aposteriori=x_apriori+K_k*y_k; */ + for (i = 0; i < 9; i++) { + y = 0.0F; + for (i0 = 0; i0 < 12; i0++) { + y += (real32_T)iv1[i + 9 * i0] * x_apriori[i0]; + } + + c_a[i] = z[i] - y; + } + + for (i = 0; i < 12; i++) { y = 0.0F; for (i0 = 0; i0 < 9; i0++) { - y += (real32_T)H_k_data[ib + udpIndVect_sizes * i0] * x_aposteriori[i0]; + y += K_k[i + 12 * i0] * c_a[i0]; } - y_k_data[ib] = z_k_data[ib] - y; + x_aposteriori[i] = x_apriori[i] + y; } - /* 'attitudeKalmanfilter:143' if updVect(4)==1 */ - if (updVect[3] == 1) { - /* 'attitudeKalmanfilter:144' if (abs(norm(z_k(4:6))-knownConst(12))>knownConst(14)) */ - for (ib = 0; ib < 3; ib++) { - x_n_b[ib] = z_k_data[ib + 3]; - } - - if ((real32_T)fabsf(norm(x_n_b) - knownConst[11]) > knownConst[13]) { - /* 'attitudeKalmanfilter:145' accUpt=10000; */ - accUpt = 10000; - } - } - - /* 'attitudeKalmanfilter:149' if updVect(7)==1 */ - if (updVect[6] == 1) { - /* 'attitudeKalmanfilter:150' if (abs(norm(z_k(7:9))-knownConst(13))>knownConst(15)) */ - for (ib = 0; ib < 3; ib++) { - x_n_b[ib] = z_k_data[ib + 6]; - } - - if ((real32_T)fabs(norm(x_n_b) - knownConst[12]) > knownConst[14]) { - /* 'attitudeKalmanfilter:152' magUpt=10000; */ - magUpt = 10000; - } - } - - /* measurement noise covariance matrix */ - /* 'attitudeKalmanfilter:157' R = [ eye(3)*rates_MeasurementNoise, zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:158' zeros(3), eye(3)*acc_MeasurementNoise*accUpt, zeros(3); */ - /* 'attitudeKalmanfilter:159' zeros(3), zeros(3), eye(3)*mag_MeasurementNoise*magUpt]; */ - /* 'attitudeKalmanfilter:161' S_k=H_k*P_apriori*H_k'+R(udpIndVect,udpIndVect); */ - /* 'attitudeKalmanfilter:162' K_k=(P_apriori*H_k'/(S_k)); */ - P_apriori_sizes[0] = 9; - P_apriori_sizes[1] = udpIndVect_sizes; - for (ib = 0; ib < 9; ib++) { - ia = udpIndVect_sizes - 1; - for (i0 = 0; i0 <= ia; i0++) { - b_A_lin[ib + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - b_A_lin[ib + 9 * i0] += P_apriori[ib + 9 * i1] * (real32_T)H_k_data[i0 - + udpIndVect_sizes * i1]; - } - } - } - - ia = udpIndVect_sizes - 1; - for (ib = 0; ib <= ia; ib++) { - for (i0 = 0; i0 < 9; i0++) { - K_k_data[ib + udpIndVect_sizes * i0] = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - K_k_data[ib + udpIndVect_sizes * i0] += (real32_T)H_k_data[ib + - udpIndVect_sizes * i1] * P_apriori[i1 + 9 * i0]; - } - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[i0 + 9 * ib] = (real32_T)iv0[i0 + 3 * ib] * knownConst[5]; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[i0 + 9 * (ib + 3)] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[i0 + 9 * (ib + 6)] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * ib) + 3] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[6] - * (real32_T)accUpt; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * ib) + 6] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F; - } - } - - for (ib = 0; ib < 3; ib++) { - for (i0 = 0; i0 < 3; i0++) { - fv0[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * knownConst[7] - * (real32_T)magUpt; - } - } - - H_k_sizes[0] = udpIndVect_sizes; - H_k_sizes[1] = udpIndVect_sizes; - ia = udpIndVect_sizes - 1; - for (ib = 0; ib <= ia; ib++) { - accUpt = udpIndVect_sizes - 1; - for (i0 = 0; i0 <= accUpt; i0++) { + /* 'attitudeKalmanfilter:107' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ + b_eye(dv1); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { y = 0.0F; for (i1 = 0; i1 < 9; i1++) { - y += K_k_data[ib + udpIndVect_sizes * i1] * (real32_T)H_k_data[i0 + - udpIndVect_sizes * i1]; + y += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0]; } - A_lin[ib + H_k_sizes[0] * i0] = y + fv0[((int32_T)udpIndVect_data[ib] + - 9 * ((int32_T)udpIndVect_data[i0] - 1)) - 1]; + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; } } - mrdivide(b_A_lin, P_apriori_sizes, A_lin, H_k_sizes, K_k_data, K_k_sizes); - - /* 'attitudeKalmanfilter:165' x_aposteriori=x_apriori+K_k*y_k; */ - if ((K_k_sizes[1] == 1) || (udpIndVect_sizes == 1)) { - for (ib = 0; ib < 9; ib++) { - b_y[ib] = 0.0F; - ia = udpIndVect_sizes - 1; - for (i0 = 0; i0 <= ia; i0++) { - b_y[ib] += K_k_data[ib + K_k_sizes[0] * i0] * y_k_data[i0]; - } - } - } else { - for (accUpt = 0; accUpt < 9; accUpt++) { - b_y[accUpt] = 0.0F; - } - - magUpt = -1; - for (ib = 0; ib + 1 <= K_k_sizes[1]; ib++) { - if ((real_T)y_k_data[ib] != 0.0) { - ia = magUpt; - for (accUpt = 0; accUpt < 9; accUpt++) { - ia++; - b_y[accUpt] += y_k_data[ib] * K_k_data[ia]; - } - } - - magUpt += 9; - } - } - - for (ib = 0; ib < 9; ib++) { - x_aposteriori[ib] += b_y[ib]; - } - - /* 'attitudeKalmanfilter:166' P_aposteriori=(eye(9)-K_k*H_k)*P_apriori; */ - b_eye(dv3); - for (ib = 0; ib < 9; ib++) { - for (i0 = 0; i0 < 9; i0++) { - y = 0.0F; - ia = K_k_sizes[1] - 1; - for (i1 = 0; i1 <= ia; i1++) { - y += K_k_data[ib + K_k_sizes[0] * i1] * (real32_T)H_k_data[i1 + - udpIndVect_sizes * i0]; - } - - fv0[ib + 9 * i0] = (real32_T)dv3[ib + 9 * i0] - y; - } - } - - for (ib = 0; ib < 9; ib++) { - for (i0 = 0; i0 < 9; i0++) { - P_aposteriori[ib + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - P_aposteriori[ib + 9 * i0] += fv0[ib + 9 * i1] * P_apriori[i1 + 9 * i0]; + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_aposteriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { - /* 'attitudeKalmanfilter:167' else */ - /* 'attitudeKalmanfilter:168' x_aposteriori=x_apriori; */ - /* 'attitudeKalmanfilter:169' P_aposteriori=P_apriori; */ - memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 81U * sizeof - (real32_T)); + /* 'attitudeKalmanfilter:108' else */ + /* 'attitudeKalmanfilter:109' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ + if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { + /* 'attitudeKalmanfilter:110' R=diag(r(1:3)); */ + c_diag(*(real32_T (*)[3])&r[0], O); + + /* observation matrix */ + /* 'attitudeKalmanfilter:113' H_k=[ E, E, Z, Z]; */ + /* 'attitudeKalmanfilter:115' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:117' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'attitudeKalmanfilter:118' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 3; i0++) { + c_P_apriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) + iv2[i1 + 12 * i0]; + } + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 12; i0++) { + fv1[i + 3 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 * + i0]; + } + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + y += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; + } + + c_a[i + 3 * i0] = y + O[i + 3 * i0]; + } + } + + b_mrdivide(c_P_apriori, c_a, S_k); + + /* 'attitudeKalmanfilter:121' x_aposteriori=x_apriori+K_k*y_k; */ + for (i = 0; i < 3; i++) { + y = 0.0F; + for (i0 = 0; i0 < 12; i0++) { + y += (real32_T)iv3[i + 3 * i0] * x_apriori[i0]; + } + + x_n_b[i] = z[i] - y; + } + + for (i = 0; i < 12; i++) { + y = 0.0F; + for (i0 = 0; i0 < 3; i0++) { + y += S_k[i + 12 * i0] * x_n_b[i0]; + } + + x_aposteriori[i] = x_apriori[i] + y; + } + + /* 'attitudeKalmanfilter:122' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ + b_eye(dv1); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + y += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0]; + } + + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_aposteriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * + i0]; + } + } + } + } else { + /* 'attitudeKalmanfilter:123' else */ + /* 'attitudeKalmanfilter:124' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ + if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) + { + /* 'attitudeKalmanfilter:125' R=diag(r(1:6)); */ + d_diag(*(real32_T (*)[6])&r[0], S_k); + + /* observation matrix */ + /* 'attitudeKalmanfilter:128' H_k=[ E, E, Z, Z; */ + /* 'attitudeKalmanfilter:129' Z, Z, E, Z]; */ + /* 'attitudeKalmanfilter:131' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:133' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:134' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 6; i0++) { + d_P_apriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) + iv4[i1 + 12 * i0]; + } + } + } + + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 12; i0++) { + b_K_k[i + 6 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + b_K_k[i + 6 * i0] += (real32_T)iv5[i + 6 * i1] * P_apriori[i1 + 12 + * i0]; + } + } + } + + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 6; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + y += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0]; + } + + fv1[i + 6 * i0] = y + S_k[i + 6 * i0]; + } + } + + c_mrdivide(d_P_apriori, fv1, b_K_k); + + /* 'attitudeKalmanfilter:137' x_aposteriori=x_apriori+K_k*y_k; */ + for (i = 0; i < 6; i++) { + y = 0.0F; + for (i0 = 0; i0 < 12; i0++) { + y += (real32_T)iv5[i + 6 * i0] * x_apriori[i0]; + } + + b_r[i] = z[i] - y; + } + + for (i = 0; i < 12; i++) { + y = 0.0F; + for (i0 = 0; i0 < 6; i0++) { + y += b_K_k[i + 12 * i0] * b_r[i0]; + } + + x_aposteriori[i] = x_apriori[i] + y; + } + + /* 'attitudeKalmanfilter:138' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + b_eye(dv1); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 6; i1++) { + y += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0]; + } + + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_aposteriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * + i0]; + } + } + } + } else { + /* 'attitudeKalmanfilter:139' else */ + /* 'attitudeKalmanfilter:140' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ + if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) + { + /* 'attitudeKalmanfilter:141' R=diag([r(1:3);r(7:9)]); */ + /* observation matrix */ + /* 'attitudeKalmanfilter:144' H_k=[ E, E, Z, Z; */ + /* 'attitudeKalmanfilter:145' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:147' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:149' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 12; i0++) { + b_K_k[i + 6 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + b_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + + 12 * i0]; + } + } + } + + for (i = 0; i < 3; i++) { + b_r[i << 1] = r[i]; + b_r[1 + (i << 1)] = r[6 + i]; + } + + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 6; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + y += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0]; + } + + S_k[i + 6 * i0] = y + b_r[3 * (i + i0)]; + } + } + + /* 'attitudeKalmanfilter:150' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 6; i0++) { + d_P_apriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) + iv7[i1 + 12 * i0]; + } + } + } + + c_mrdivide(d_P_apriori, S_k, b_K_k); + + /* 'attitudeKalmanfilter:153' x_aposteriori=x_apriori+K_k*y_k; */ + for (i = 0; i < 3; i++) { + b_r[i] = z[i]; + } + + for (i = 0; i < 3; i++) { + b_r[i + 3] = z[i + 6]; + } + + for (i = 0; i < 6; i++) { + fv2[i] = 0.0F; + for (i0 = 0; i0 < 12; i0++) { + fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; + } + + b_z[i] = b_r[i] - fv2[i]; + } + + for (i = 0; i < 12; i++) { + y = 0.0F; + for (i0 = 0; i0 < 6; i0++) { + y += b_K_k[i + 12 * i0] * b_z[i0]; + } + + x_aposteriori[i] = x_apriori[i] + y; + } + + /* 'attitudeKalmanfilter:154' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + b_eye(dv1); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + y = 0.0F; + for (i1 = 0; i1 < 6; i1++) { + y += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; + } + + Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_aposteriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 + * i0]; + } + } + } + } else { + /* 'attitudeKalmanfilter:155' else */ + /* 'attitudeKalmanfilter:156' x_aposteriori=x_apriori; */ + for (i = 0; i < 12; i++) { + x_aposteriori[i] = x_apriori[i]; + } + + /* 'attitudeKalmanfilter:157' P_aposteriori=P_apriori; */ + memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 144U * sizeof + (real32_T)); + } + } + } } - /* %% euler anglels extraction */ - /* 'attitudeKalmanfilter:175' z_n_b = -x_aposteriori(4:6)./norm(x_aposteriori(4:6)); */ - y = norm(*(real32_T (*)[3])&x_aposteriori[3]); + /* % euler anglels extraction */ + /* 'attitudeKalmanfilter:166' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ + y = norm(*(real32_T (*)[3])&x_aposteriori[6]); - /* 'attitudeKalmanfilter:176' m_n_b = x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ - c_y = norm(*(real32_T (*)[3])&x_aposteriori[6]); + /* 'attitudeKalmanfilter:167' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ + b_y = norm(*(real32_T (*)[3])&x_aposteriori[9]); - /* 'attitudeKalmanfilter:178' y_n_b=cross(z_n_b,m_n_b); */ - for (accUpt = 0; accUpt < 3; accUpt++) { - z_n_b[accUpt] = -x_aposteriori[accUpt + 3] / y; - x_n_b[accUpt] = x_aposteriori[accUpt + 6] / c_y; + /* 'attitudeKalmanfilter:169' y_n_b=cross(z_n_b,m_n_b); */ + for (i = 0; i < 3; i++) { + z_n_b[i] = -x_aposteriori[i + 6] / y; + x_n_b[i] = x_aposteriori[i + 9] / b_y; } cross(z_n_b, x_n_b, y_n_b); - /* 'attitudeKalmanfilter:179' y_n_b=y_n_b./norm(y_n_b); */ + /* 'attitudeKalmanfilter:170' y_n_b=y_n_b./norm(y_n_b); */ y = norm(y_n_b); - for (ib = 0; ib < 3; ib++) { - y_n_b[ib] /= y; + for (i = 0; i < 3; i++) { + y_n_b[i] /= y; } - /* 'attitudeKalmanfilter:181' x_n_b=(cross(y_n_b,z_n_b)); */ + /* 'attitudeKalmanfilter:172' x_n_b=(cross(y_n_b,z_n_b)); */ cross(y_n_b, z_n_b, x_n_b); - /* 'attitudeKalmanfilter:182' x_n_b=x_n_b./norm(x_n_b); */ + /* 'attitudeKalmanfilter:173' x_n_b=x_n_b./norm(x_n_b); */ y = norm(x_n_b); - for (ib = 0; ib < 3; ib++) { - /* 'attitudeKalmanfilter:188' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ - Rot_matrix[ib] = x_n_b[ib] / y; - Rot_matrix[3 + ib] = y_n_b[ib]; - Rot_matrix[6 + ib] = z_n_b[ib]; + for (i = 0; i < 3; i++) { + /* 'attitudeKalmanfilter:179' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + Rot_matrix[i] = x_n_b[i] / y; + Rot_matrix[3 + i] = y_n_b[i]; + Rot_matrix[6 + i] = z_n_b[i]; } - /* 'attitudeKalmanfilter:192' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:193' theta=-asin(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:194' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */ + /* 'attitudeKalmanfilter:183' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'attitudeKalmanfilter:184' theta=-asin(Rot_matrix(1,3)); */ + /* 'attitudeKalmanfilter:185' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'attitudeKalmanfilter:186' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index 56f02b4c81..f8f99fa5a3 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:43 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ @@ -29,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[9], real32_T P_aposteriori[81]); +extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); #endif /* End of code generation (attitudeKalmanfilter.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c index b72256a09e..689bc49e94 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index efb465b255..dcce3cd728 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c index d0bf625f09..39f8f648c8 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index 1ad84575f8..ea7b9e03ea 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h index bd83cc168a..6d5704a7a8 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:42 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c index c888694536..27da4b6b93 100755 --- a/apps/attitude_estimator_ekf/codegen/cross.c +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -3,7 +3,7 @@ * * Code generation for function 'cross' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h index 92e3a884d0..8ef2c475c2 100755 --- a/apps/attitude_estimator_ekf/codegen/cross.h +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -3,7 +3,7 @@ * * Code generation for function 'cross' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c index 522f6e2851..81626589d2 100755 --- a/apps/attitude_estimator_ekf/codegen/diag.c +++ b/apps/attitude_estimator_ekf/codegen/diag.c @@ -3,7 +3,7 @@ * * Code generation for function 'diag' * - * C source code generated on: Fri Sep 21 13:56:43 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ @@ -27,7 +27,19 @@ /* * */ -void diag(const real32_T v[3], real32_T d[9]) +void b_diag(const real32_T v[9], real32_T d[81]) +{ + int32_T j; + memset((void *)&d[0], 0, 81U * sizeof(real32_T)); + for (j = 0; j < 9; j++) { + d[j + 9 * j] = v[j]; + } +} + +/* + * + */ +void c_diag(const real32_T v[3], real32_T d[9]) { int32_T j; for (j = 0; j < 9; j++) { @@ -39,4 +51,28 @@ void diag(const real32_T v[3], real32_T d[9]) } } +/* + * + */ +void d_diag(const real32_T v[6], real32_T d[36]) +{ + int32_T j; + memset((void *)&d[0], 0, 36U * sizeof(real32_T)); + for (j = 0; j < 6; j++) { + d[j + 6 * j] = v[j]; + } +} + +/* + * + */ +void diag(const real32_T v[12], real32_T d[144]) +{ + int32_T j; + memset((void *)&d[0], 0, 144U * sizeof(real32_T)); + for (j = 0; j < 12; j++) { + d[j + 12 * j] = v[j]; + } +} + /* End of code generation (diag.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h index bb92fb2424..10cea81b24 100755 --- a/apps/attitude_estimator_ekf/codegen/diag.h +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -3,7 +3,7 @@ * * Code generation for function 'diag' * - * C source code generated on: Fri Sep 21 13:56:43 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ @@ -29,6 +29,9 @@ /* Variable Definitions */ /* Function Declarations */ -extern void diag(const real32_T v[3], real32_T d[9]); +extern void b_diag(const real32_T v[9], real32_T d[81]); +extern void c_diag(const real32_T v[3], real32_T d[9]); +extern void d_diag(const real32_T v[6], real32_T d[36]); +extern void diag(const real32_T v[12], real32_T d[144]); #endif /* End of code generation (diag.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c index e67071dcea..2db070e6fe 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Fri Sep 21 13:56:43 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ @@ -27,12 +27,12 @@ /* * */ -void b_eye(real_T I[81]) +void b_eye(real_T I[144]) { int32_T i; - memset((void *)&I[0], 0, 81U * sizeof(real_T)); - for (i = 0; i < 9; i++) { - I[i + 9 * i] = 1.0; + memset((void *)&I[0], 0, 144U * sizeof(real_T)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1.0; } } diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h index c07a1bc970..027db1c065 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Fri Sep 21 13:56:43 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ @@ -29,7 +29,7 @@ /* Variable Definitions */ /* Function Declarations */ -extern void b_eye(real_T I[81]); +extern void b_eye(real_T I[144]); extern void eye(real_T I[9]); #endif /* End of code generation (eye.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/find.c b/apps/attitude_estimator_ekf/codegen/find.c deleted file mode 100755 index 8f05ef146e..0000000000 --- a/apps/attitude_estimator_ekf/codegen/find.c +++ /dev/null @@ -1,77 +0,0 @@ -/* - * find.c - * - * Code generation for function 'find' - * - * C source code generated on: Fri Sep 21 13:56:43 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "find.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]) -{ - int32_T idx; - int32_T ii; - boolean_T exitg1; - boolean_T guard1 = FALSE; - int32_T i2; - int8_T b_i_data[9]; - idx = 0; - i_sizes[0] = 9; - ii = 1; - exitg1 = 0U; - while ((exitg1 == 0U) && (ii <= 9)) { - guard1 = FALSE; - if (x[ii - 1] != 0) { - idx++; - i_data[idx - 1] = (real_T)ii; - if (idx >= 9) { - exitg1 = 1U; - } else { - guard1 = TRUE; - } - } else { - guard1 = TRUE; - } - - if (guard1 == TRUE) { - ii++; - } - } - - if (1 > idx) { - idx = 0; - } - - ii = idx - 1; - for (i2 = 0; i2 <= ii; i2++) { - b_i_data[i2] = (int8_T)i_data[i2]; - } - - i_sizes[0] = idx; - ii = idx - 1; - for (i2 = 0; i2 <= ii; i2++) { - i_data[i2] = (real_T)b_i_data[i2]; - } -} - -/* End of code generation (find.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c index d098162d54..ce29e42cd0 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ @@ -21,719 +21,345 @@ /* Variable Definitions */ /* Function Declarations */ -static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x); -static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2], - real32_T B_data[81], int32_T B_sizes[2]); -static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data - [81], int32_T x_sizes[2], int32_T ix0); -static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2], - real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes - [2]); -static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T - x_sizes[2], int32_T ix0); /* Function Definitions */ /* * */ -static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x) +void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) { - return 0.0F; + int32_T rtemp; + int32_T k; + real32_T b_A[9]; + real32_T b_B[36]; + int32_T r1; + int32_T r2; + int32_T r3; + real32_T maxval; + real32_T a21; + real32_T Y[36]; + for (rtemp = 0; rtemp < 3; rtemp++) { + for (k = 0; k < 3; k++) { + b_A[k + 3 * rtemp] = B[rtemp + 3 * k]; + } + } + + for (rtemp = 0; rtemp < 12; rtemp++) { + for (k = 0; k < 3; k++) { + b_B[k + 3 * rtemp] = A[rtemp + 12 * k]; + } + } + + r1 = 0; + r2 = 1; + r3 = 2; + maxval = (real32_T)fabsf(b_A[0]); + a21 = (real32_T)fabsf(b_A[1]); + if (a21 > maxval) { + maxval = a21; + r1 = 1; + r2 = 0; + } + + if ((real32_T)fabsf(b_A[2]) > maxval) { + r1 = 2; + r2 = 1; + r3 = 0; + } + + b_A[r2] /= b_A[r1]; + b_A[r3] /= b_A[r1]; + b_A[3 + r2] -= b_A[r2] * b_A[3 + r1]; + b_A[3 + r3] -= b_A[r3] * b_A[3 + r1]; + b_A[6 + r2] -= b_A[r2] * b_A[6 + r1]; + b_A[6 + r3] -= b_A[r3] * b_A[6 + r1]; + if ((real32_T)fabsf(b_A[3 + r3]) > (real32_T)fabsf(b_A[3 + r2])) { + rtemp = r2; + r2 = r3; + r3 = rtemp; + } + + b_A[3 + r3] /= b_A[3 + r2]; + b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2]; + for (k = 0; k < 12; k++) { + Y[3 * k] = b_B[r1 + 3 * k]; + Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2]; + Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3 + + r3]; + Y[2 + 3 * k] /= b_A[6 + r3]; + Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1]; + Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2]; + Y[1 + 3 * k] /= b_A[3 + r2]; + Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1]; + Y[3 * k] /= b_A[r1]; + } + + for (rtemp = 0; rtemp < 3; rtemp++) { + for (k = 0; k < 12; k++) { + y[k + 12 * rtemp] = Y[rtemp + 3 * k]; + } + } } /* * */ -static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2], - real32_T B_data[81], int32_T B_sizes[2]) +void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) { - int32_T loop_ub; + int32_T jy; int32_T iy; - real32_T b_A_data[81]; - int32_T jA; - int32_T tmp_data[9]; - int32_T ipiv_data[9]; - int32_T ldap1; + real32_T b_A[36]; + int8_T ipiv[6]; int32_T j; int32_T mmj; int32_T jj; + int32_T jp1j; + int32_T c; int32_T ix; real32_T temp; int32_T k; real32_T s; - int32_T jrow; - int32_T b_loop_ub; - int32_T c; - loop_ub = A_sizes[0] * A_sizes[1] - 1; - for (iy = 0; iy <= loop_ub; iy++) { - b_A_data[iy] = A_data[iy]; - } - - iy = A_sizes[1]; - jA = A_sizes[1]; - jA = iy <= jA ? iy : jA; - if (jA < 1) { - } else { - loop_ub = jA - 1; - for (iy = 0; iy <= loop_ub; iy++) { - tmp_data[iy] = 1 + iy; + int32_T loop_ub; + real32_T Y[72]; + for (jy = 0; jy < 6; jy++) { + for (iy = 0; iy < 6; iy++) { + b_A[iy + 6 * jy] = B[jy + 6 * iy]; } - loop_ub = jA - 1; - for (iy = 0; iy <= loop_ub; iy++) { - ipiv_data[iy] = tmp_data[iy]; - } + ipiv[jy] = (int8_T)(1 + jy); } - ldap1 = A_sizes[1] + 1; - iy = A_sizes[1] - 1; - jA = A_sizes[1]; - loop_ub = iy <= jA ? iy : jA; - for (j = 1; j <= loop_ub; j++) { - mmj = (A_sizes[1] - j) + 1; - jj = (j - 1) * ldap1; - if (mmj < 1) { - jA = -1; - } else { - jA = 0; - if (mmj > 1) { - ix = jj; - temp = (real32_T)fabs(b_A_data[jj]); - for (k = 1; k + 1 <= mmj; k++) { - ix++; - s = (real32_T)fabs(b_A_data[ix]); - if (s > temp) { - jA = k; - temp = s; - } - } + for (j = 0; j < 5; j++) { + mmj = -j; + jj = j * 7; + jp1j = jj + 1; + c = mmj + 6; + jy = 0; + ix = jj; + temp = (real32_T)fabsf(b_A[jj]); + for (k = 2; k <= c; k++) { + ix++; + s = (real32_T)fabsf(b_A[ix]); + if (s > temp) { + jy = k - 1; + temp = s; } } - if ((real_T)b_A_data[jj + jA] != 0.0) { - if (jA != 0) { - ipiv_data[j - 1] = j + jA; - jrow = j - 1; - iy = jrow + jA; - for (k = 1; k <= A_sizes[1]; k++) { - temp = b_A_data[jrow]; - b_A_data[jrow] = b_A_data[iy]; - b_A_data[iy] = temp; - jrow += A_sizes[1]; - iy += A_sizes[1]; + if ((real_T)b_A[jj + jy] != 0.0) { + if (jy != 0) { + ipiv[j] = (int8_T)((j + jy) + 1); + ix = j; + iy = j + jy; + for (k = 0; k < 6; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 6; + iy += 6; } } - b_loop_ub = jj + mmj; - for (jA = jj + 1; jA + 1 <= b_loop_ub; jA++) { - b_A_data[jA] /= b_A_data[jj]; + loop_ub = (jp1j + mmj) + 5; + for (iy = jp1j; iy + 1 <= loop_ub; iy++) { + b_A[iy] /= b_A[jj]; } } - c = A_sizes[1] - j; - jA = jj + ldap1; - iy = jj + A_sizes[1]; - for (jrow = 1; jrow <= c; jrow++) { - if ((real_T)b_A_data[iy] != 0.0) { - temp = b_A_data[iy] * -1.0F; - ix = jj; - b_loop_ub = (mmj + jA) - 1; - for (k = jA; k + 1 <= b_loop_ub; k++) { - b_A_data[k] += b_A_data[ix + 1] * temp; + c = 5 - j; + jy = jj + 6; + for (iy = 1; iy <= c; iy++) { + if ((real_T)b_A[jy] != 0.0) { + temp = b_A[jy] * -1.0F; + ix = jp1j; + loop_ub = (mmj + jj) + 12; + for (k = 7 + jj; k + 1 <= loop_ub; k++) { + b_A[k] += b_A[ix] * temp; ix++; } } - iy += A_sizes[1]; - jA += A_sizes[1]; + jy += 6; + jj += 6; } } - for (jA = 0; jA + 1 <= A_sizes[1]; jA++) { - if (ipiv_data[jA] != jA + 1) { - for (j = 0; j < 9; j++) { - temp = B_data[jA + B_sizes[0] * j]; - B_data[jA + B_sizes[0] * j] = B_data[(ipiv_data[jA] + B_sizes[0] * j) - - 1]; - B_data[(ipiv_data[jA] + B_sizes[0] * j) - 1] = temp; + for (jy = 0; jy < 12; jy++) { + for (iy = 0; iy < 6; iy++) { + Y[iy + 6 * jy] = A[jy + 12 * iy]; + } + } + + for (iy = 0; iy < 6; iy++) { + if (ipiv[iy] != iy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[iy + 6 * j]; + Y[iy + 6 * j] = Y[(ipiv[iy] + 6 * j) - 1]; + Y[(ipiv[iy] + 6 * j) - 1] = temp; } } } - if (B_sizes[0] == 0) { - } else { - for (j = 0; j < 9; j++) { - c = A_sizes[1] * j; - for (k = 0; k + 1 <= A_sizes[1]; k++) { - iy = A_sizes[1] * k; - if ((real_T)B_data[k + c] != 0.0) { - for (jA = k + 1; jA + 1 <= A_sizes[1]; jA++) { - B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy]; - } + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 0; k < 6; k++) { + jy = 6 * k; + if ((real_T)Y[k + c] != 0.0) { + for (iy = k + 2; iy < 7; iy++) { + Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1]; } } } } - if (B_sizes[0] == 0) { - } else { - for (j = 0; j < 9; j++) { - c = A_sizes[1] * j; - for (k = A_sizes[1] - 1; k + 1 > 0; k--) { - iy = A_sizes[1] * k; - if ((real_T)B_data[k + c] != 0.0) { - B_data[k + c] /= b_A_data[k + iy]; - for (jA = 0; jA + 1 <= k; jA++) { - B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy]; - } + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 5; k > -1; k += -1) { + jy = 6 * k; + if ((real_T)Y[k + c] != 0.0) { + Y[k + c] /= b_A[k + jy]; + for (iy = 0; iy + 1 <= k; iy++) { + Y[iy + c] -= Y[k + c] * b_A[iy + jy]; } } } } + + for (jy = 0; jy < 6; jy++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * jy] = Y[jy + 6 * iy]; + } + } } /* * */ -static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data - [81], int32_T x_sizes[2], int32_T ix0) +void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) { - real32_T tau; - int32_T nm1; - real32_T xnorm; - real32_T a; - int32_T knt; - int32_T loop_ub; - int32_T k; - tau = 0.0F; - if (n <= 0) { - } else { - nm1 = n - 2; - xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0); - if ((real_T)xnorm != 0.0) { - a = (real32_T)fabs(*alpha1); - xnorm = (real32_T)fabs(xnorm); - if (a < xnorm) { - a /= xnorm; - xnorm *= (real32_T)sqrt(a * a + 1.0F); - } else if (a > xnorm) { - xnorm /= a; - xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a; - } else if (rtIsNaNF(xnorm)) { - } else { - xnorm = a * 1.41421354F; - } - - if ((real_T)*alpha1 >= 0.0) { - xnorm = -xnorm; - } - - if ((real32_T)fabs(xnorm) < 9.86076132E-32F) { - knt = 0; - do { - knt++; - loop_ub = ix0 + nm1; - for (k = ix0; k <= loop_ub; k++) { - x_data[k - 1] *= 1.01412048E+31F; - } - - xnorm *= 1.01412048E+31F; - *alpha1 *= 1.01412048E+31F; - } while (!((real32_T)fabs(xnorm) >= 9.86076132E-32F)); - - xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0); - a = (real32_T)fabs(*alpha1); - xnorm = (real32_T)fabs(xnorm); - if (a < xnorm) { - a /= xnorm; - xnorm *= (real32_T)sqrt(a * a + 1.0F); - } else if (a > xnorm) { - xnorm /= a; - xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a; - } else if (rtIsNaNF(xnorm)) { - } else { - xnorm = a * 1.41421354F; - } - - if ((real_T)*alpha1 >= 0.0) { - xnorm = -xnorm; - } - - tau = (xnorm - *alpha1) / xnorm; - *alpha1 = 1.0F / (*alpha1 - xnorm); - loop_ub = ix0 + nm1; - for (k = ix0; k <= loop_ub; k++) { - x_data[k - 1] *= *alpha1; - } - - for (k = 1; k <= knt; k++) { - xnorm *= 9.86076132E-32F; - } - - *alpha1 = xnorm; - } else { - tau = (xnorm - *alpha1) / xnorm; - *alpha1 = 1.0F / (*alpha1 - xnorm); - loop_ub = ix0 + nm1; - for (k = ix0; k <= loop_ub; k++) { - x_data[k - 1] *= *alpha1; - } - - *alpha1 = xnorm; - } - } - } - - return tau; -} - -/* - * - */ -static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2], - real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes - [2]) -{ - real_T rankR; - real_T u1; - int32_T mn; - int32_T b_A_sizes[2]; - int32_T loop_ub; - int32_T k; - real32_T b_A_data[81]; - int32_T pvt; - int32_T b_mn; - int32_T tmp_data[9]; - int32_T jpvt_data[9]; - int8_T unnamed_idx_0; - real32_T work_data[9]; - int32_T nmi; - real32_T vn1_data[9]; - real32_T vn2_data[9]; - int32_T i; - int32_T i_i; - int32_T mmi; - int32_T ix; - real32_T smax; - real32_T s; + int32_T jy; int32_T iy; - real32_T atmp; - real32_T tau_data[9]; - int32_T i_ip1; - int32_T lastv; - int32_T lastc; - boolean_T exitg2; - int32_T exitg1; - boolean_T firstNonZero; - real32_T t; - rankR = (real_T)A_sizes[0]; - u1 = (real_T)A_sizes[1]; - rankR = rankR <= u1 ? rankR : u1; - mn = (int32_T)rankR; - b_A_sizes[0] = A_sizes[0]; - b_A_sizes[1] = A_sizes[1]; - loop_ub = A_sizes[0] * A_sizes[1] - 1; - for (k = 0; k <= loop_ub; k++) { - b_A_data[k] = A_data[k]; - } - - k = A_sizes[0]; - pvt = A_sizes[1]; - b_mn = k <= pvt ? k : pvt; - if (A_sizes[1] < 1) { - } else { - loop_ub = A_sizes[1] - 1; - for (k = 0; k <= loop_ub; k++) { - tmp_data[k] = 1 + k; - } - - loop_ub = A_sizes[1] - 1; - for (k = 0; k <= loop_ub; k++) { - jpvt_data[k] = tmp_data[k]; - } - } - - if ((A_sizes[0] == 0) || (A_sizes[1] == 0)) { - } else { - unnamed_idx_0 = (int8_T)A_sizes[1]; - loop_ub = unnamed_idx_0 - 1; - for (k = 0; k <= loop_ub; k++) { - work_data[k] = 0.0F; - } - - k = 1; - for (nmi = 0; nmi + 1 <= A_sizes[1]; nmi++) { - vn1_data[nmi] = eml_xnrm2(A_sizes[0], A_data, A_sizes, k); - vn2_data[nmi] = vn1_data[nmi]; - k += A_sizes[0]; - } - - for (i = 0; i + 1 <= b_mn; i++) { - i_i = i + i * A_sizes[0]; - nmi = A_sizes[1] - i; - mmi = (A_sizes[0] - i) - 1; - if (nmi < 1) { - pvt = -1; - } else { - pvt = 0; - if (nmi > 1) { - ix = i; - smax = (real32_T)fabs(vn1_data[i]); - for (k = 1; k + 1 <= nmi; k++) { - ix++; - s = (real32_T)fabs(vn1_data[ix]); - if (s > smax) { - pvt = k; - smax = s; - } - } - } - } - - pvt += i; - if (pvt + 1 != i + 1) { - ix = A_sizes[0] * pvt; - iy = A_sizes[0] * i; - for (k = 1; k <= A_sizes[0]; k++) { - s = b_A_data[ix]; - b_A_data[ix] = b_A_data[iy]; - b_A_data[iy] = s; - ix++; - iy++; - } - - k = jpvt_data[pvt]; - jpvt_data[pvt] = jpvt_data[i]; - jpvt_data[i] = k; - vn1_data[pvt] = vn1_data[i]; - vn2_data[pvt] = vn2_data[i]; - } - - if (i + 1 < A_sizes[0]) { - atmp = b_A_data[i_i]; - smax = eml_matlab_zlarfg(mmi + 1, &atmp, b_A_data, b_A_sizes, i_i + 2); - tau_data[i] = smax; - } else { - atmp = b_A_data[i_i]; - smax = b_A_data[i_i]; - s = b_eml_matlab_zlarfg(&atmp, &smax); - b_A_data[i_i] = smax; - tau_data[i] = s; - } - - b_A_data[i_i] = atmp; - if (i + 1 < A_sizes[1]) { - atmp = b_A_data[i_i]; - b_A_data[i_i] = 1.0F; - i_ip1 = (i + (i + 1) * A_sizes[0]) + 1; - if ((real_T)tau_data[i] != 0.0) { - lastv = mmi; - pvt = i_i + mmi; - while ((lastv + 1 > 0) && ((real_T)b_A_data[pvt] == 0.0)) { - lastv--; - pvt--; - } - - lastc = nmi - 1; - exitg2 = 0U; - while ((exitg2 == 0U) && (lastc > 0)) { - k = i_ip1 + (lastc - 1) * A_sizes[0]; - pvt = k + lastv; - do { - exitg1 = 0U; - if (k <= pvt) { - if ((real_T)b_A_data[k - 1] != 0.0) { - exitg1 = 1U; - } else { - k++; - } - } else { - lastc--; - exitg1 = 2U; - } - } while (exitg1 == 0U); - - if (exitg1 == 1U) { - exitg2 = 1U; - } - } - } else { - lastv = -1; - lastc = 0; - } - - if (lastv + 1 > 0) { - if (lastc == 0) { - } else { - for (iy = 1; iy <= lastc; iy++) { - work_data[iy - 1] = 0.0F; - } - - iy = 0; - loop_ub = i_ip1 + A_sizes[0] * (lastc - 1); - pvt = i_ip1; - while ((A_sizes[0] > 0) && (pvt <= loop_ub)) { - ix = i_i; - smax = 0.0F; - k = pvt + lastv; - for (nmi = pvt; nmi <= k; nmi++) { - smax += b_A_data[nmi - 1] * b_A_data[ix]; - ix++; - } - - work_data[iy] += smax; - iy++; - pvt += A_sizes[0]; - } - } - - smax = -tau_data[i]; - if ((real_T)smax == 0.0) { - } else { - pvt = 0; - for (nmi = 1; nmi <= lastc; nmi++) { - if ((real_T)work_data[pvt] != 0.0) { - s = work_data[pvt] * smax; - ix = i_i; - loop_ub = lastv + i_ip1; - for (k = i_ip1 - 1; k + 1 <= loop_ub; k++) { - b_A_data[k] += b_A_data[ix] * s; - ix++; - } - } - - pvt++; - i_ip1 += A_sizes[0]; - } - } - } - - b_A_data[i_i] = atmp; - } - - for (nmi = i + 1; nmi + 1 <= A_sizes[1]; nmi++) { - if ((real_T)vn1_data[nmi] != 0.0) { - smax = (real32_T)fabs(b_A_data[i + b_A_sizes[0] * nmi]) / vn1_data[nmi]; - smax = 1.0F - smax * smax; - if ((real_T)smax < 0.0) { - smax = 0.0F; - } - - s = vn1_data[nmi] / vn2_data[nmi]; - if (smax * (s * s) <= 0.000345266977F) { - if (i + 1 < A_sizes[0]) { - k = (i + A_sizes[0] * nmi) + 1; - smax = 0.0F; - if (mmi < 1) { - } else if (mmi == 1) { - smax = (real32_T)fabs(b_A_data[k]); - } else { - s = 0.0F; - firstNonZero = TRUE; - pvt = k + mmi; - while (k + 1 <= pvt) { - if (b_A_data[k] != 0.0F) { - atmp = (real32_T)fabs(b_A_data[k]); - if (firstNonZero) { - s = atmp; - smax = 1.0F; - firstNonZero = FALSE; - } else if (s < atmp) { - t = s / atmp; - smax = 1.0F + smax * t * t; - s = atmp; - } else { - t = atmp / s; - smax += t * t; - } - } - - k++; - } - - smax = s * (real32_T)sqrt(smax); - } - - vn1_data[nmi] = smax; - vn2_data[nmi] = vn1_data[nmi]; - } else { - vn1_data[nmi] = 0.0F; - vn2_data[nmi] = 0.0F; - } - } else { - vn1_data[nmi] *= (real32_T)sqrt(smax); - } - } - } - } - } - - rankR = (real_T)A_sizes[0]; - u1 = (real_T)A_sizes[1]; - rankR = rankR >= u1 ? rankR : u1; - smax = (real32_T)rankR * (real32_T)fabs(b_A_data[0]) * 1.1920929E-7F; - rankR = 0.0; - k = 0; - while ((k + 1 <= mn) && (!((real32_T)fabs(b_A_data[k + b_A_sizes[0] * k]) <= - smax))) { - rankR++; - k++; - } - - unnamed_idx_0 = (int8_T)A_sizes[1]; - Y_sizes[0] = (int32_T)unnamed_idx_0; - Y_sizes[1] = 9; - loop_ub = unnamed_idx_0 * 9 - 1; - for (k = 0; k <= loop_ub; k++) { - Y_data[k] = 0.0F; - } - - for (nmi = 0; nmi + 1 <= mn; nmi++) { - if ((real_T)tau_data[nmi] != 0.0) { - for (k = 0; k < 9; k++) { - smax = B_data[nmi + B_sizes[0] * k]; - for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) { - smax += b_A_data[i + b_A_sizes[0] * nmi] * B_data[i + B_sizes[0] * k]; - } - - smax *= tau_data[nmi]; - if ((real_T)smax != 0.0) { - B_data[nmi + B_sizes[0] * k] -= smax; - for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) { - B_data[i + B_sizes[0] * k] -= b_A_data[i + b_A_sizes[0] * nmi] * - smax; - } - } - } - } - } - - for (k = 0; k < 9; k++) { - for (i = 0; (real_T)(i + 1) <= rankR; i++) { - Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] = B_data[i + B_sizes[0] * k]; - } - - for (nmi = (int32_T)rankR - 1; nmi + 1 >= 1; nmi--) { - Y_data[(jpvt_data[nmi] + Y_sizes[0] * k) - 1] /= b_A_data[nmi + b_A_sizes - [0] * nmi]; - for (i = 0; i + 1 <= nmi; i++) { - Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] -= Y_data[(jpvt_data[nmi] + - Y_sizes[0] * k) - 1] * b_A_data[i + b_A_sizes[0] * nmi]; - } - } - } -} - -/* - * - */ -static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T - x_sizes[2], int32_T ix0) -{ - real32_T y; - real32_T scale; - boolean_T firstNonZero; - int32_T kend; + real32_T b_A[81]; + int8_T ipiv[9]; + int32_T j; + int32_T mmj; + int32_T jj; + int32_T jp1j; + int32_T c; + int32_T ix; + real32_T temp; int32_T k; - real32_T absxk; - real32_T t; - y = 0.0F; - if (n < 1) { - } else if (n == 1) { - y = (real32_T)fabs(x_data[ix0 - 1]); - } else { - scale = 0.0F; - firstNonZero = TRUE; - kend = (ix0 + n) - 1; - for (k = ix0 - 1; k + 1 <= kend; k++) { - if (x_data[k] != 0.0F) { - absxk = (real32_T)fabs(x_data[k]); - if (firstNonZero) { - scale = absxk; - y = 1.0F; - firstNonZero = FALSE; - } else if (scale < absxk) { - t = scale / absxk; - y = 1.0F + y * t * t; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } + real32_T s; + int32_T loop_ub; + real32_T Y[108]; + for (jy = 0; jy < 9; jy++) { + for (iy = 0; iy < 9; iy++) { + b_A[iy + 9 * jy] = B[jy + 9 * iy]; + } + + ipiv[jy] = (int8_T)(1 + jy); + } + + for (j = 0; j < 8; j++) { + mmj = -j; + jj = j * 10; + jp1j = jj + 1; + c = mmj + 9; + jy = 0; + ix = jj; + temp = (real32_T)fabsf(b_A[jj]); + for (k = 2; k <= c; k++) { + ix++; + s = (real32_T)fabsf(b_A[ix]); + if (s > temp) { + jy = k - 1; + temp = s; } } - y = scale * (real32_T)sqrt(y); - } + if ((real_T)b_A[jj + jy] != 0.0) { + if (jy != 0) { + ipiv[j] = (int8_T)((j + jy) + 1); + ix = j; + iy = j + jy; + for (k = 0; k < 9; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 9; + iy += 9; + } + } - return y; -} + loop_ub = (jp1j + mmj) + 8; + for (iy = jp1j; iy + 1 <= loop_ub; iy++) { + b_A[iy] /= b_A[jj]; + } + } -/* - * - */ -void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const - real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], - int32_T y_sizes[2]) -{ - int32_T b_A_sizes[2]; - int32_T loop_ub; - int32_T i3; - int32_T b_loop_ub; - int32_T i4; - real32_T b_A_data[81]; - int32_T b_B_sizes[2]; - real32_T b_B_data[81]; - int8_T unnamed_idx_0; - int32_T c_B_sizes[2]; - real32_T c_B_data[81]; - b_A_sizes[0] = B_sizes[1]; - b_A_sizes[1] = B_sizes[0]; - loop_ub = B_sizes[0] - 1; - for (i3 = 0; i3 <= loop_ub; i3++) { - b_loop_ub = B_sizes[1] - 1; - for (i4 = 0; i4 <= b_loop_ub; i4++) { - b_A_data[i4 + b_A_sizes[0] * i3] = B_data[i3 + B_sizes[0] * i4]; + c = 8 - j; + jy = jj + 9; + for (iy = 1; iy <= c; iy++) { + if ((real_T)b_A[jy] != 0.0) { + temp = b_A[jy] * -1.0F; + ix = jp1j; + loop_ub = (mmj + jj) + 18; + for (k = 10 + jj; k + 1 <= loop_ub; k++) { + b_A[k] += b_A[ix] * temp; + ix++; + } + } + + jy += 9; + jj += 9; } } - b_B_sizes[0] = A_sizes[1]; - b_B_sizes[1] = 9; - for (i3 = 0; i3 < 9; i3++) { - loop_ub = A_sizes[1] - 1; - for (i4 = 0; i4 <= loop_ub; i4++) { - b_B_data[i4 + b_B_sizes[0] * i3] = A_data[i3 + A_sizes[0] * i4]; + for (jy = 0; jy < 12; jy++) { + for (iy = 0; iy < 9; iy++) { + Y[iy + 9 * jy] = A[jy + 12 * iy]; } } - if ((b_A_sizes[0] == 0) || (b_A_sizes[1] == 0) || (b_B_sizes[0] == 0)) { - unnamed_idx_0 = (int8_T)b_A_sizes[1]; - b_B_sizes[0] = (int32_T)unnamed_idx_0; - b_B_sizes[1] = 9; - loop_ub = unnamed_idx_0 * 9 - 1; - for (i3 = 0; i3 <= loop_ub; i3++) { - b_B_data[i3] = 0.0F; + for (iy = 0; iy < 9; iy++) { + if (ipiv[iy] != iy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[iy + 9 * j]; + Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1]; + Y[(ipiv[iy] + 9 * j) - 1] = temp; + } } - } else if (b_A_sizes[0] == b_A_sizes[1]) { - eml_lusolve(b_A_data, b_A_sizes, b_B_data, b_B_sizes); - } else { - c_B_sizes[0] = b_B_sizes[0]; - c_B_sizes[1] = 9; - loop_ub = b_B_sizes[0] * 9 - 1; - for (i3 = 0; i3 <= loop_ub; i3++) { - c_B_data[i3] = b_B_data[i3]; - } - - eml_qrsolve(b_A_data, b_A_sizes, c_B_data, c_B_sizes, b_B_data, b_B_sizes); } - y_sizes[0] = 9; - y_sizes[1] = b_B_sizes[0]; - loop_ub = b_B_sizes[0] - 1; - for (i3 = 0; i3 <= loop_ub; i3++) { - for (i4 = 0; i4 < 9; i4++) { - y_data[i4 + 9 * i3] = b_B_data[i3 + b_B_sizes[0] * i4]; + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 0; k < 9; k++) { + jy = 9 * k; + if ((real_T)Y[k + c] != 0.0) { + for (iy = k + 2; iy < 10; iy++) { + Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 8; k > -1; k += -1) { + jy = 9 * k; + if ((real_T)Y[k + c] != 0.0) { + Y[k + c] /= b_A[k + jy]; + for (iy = 0; iy + 1 <= k; iy++) { + Y[iy + c] -= Y[k + c] * b_A[iy + jy]; + } + } + } + } + + for (jy = 0; jy < 9; jy++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * jy] = Y[jy + 9 * iy]; } } } diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h index e807afcc8b..b80f34357f 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ @@ -29,6 +29,8 @@ /* Variable Definitions */ /* Function Declarations */ -extern void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], int32_T y_sizes[2]); +extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]); +extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]); +extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); #endif /* End of code generation (mrdivide.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index fa07e1a90e..341c930220 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Fri Sep 21 13:56:43 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h index 63294717f4..0f58dbe694 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/power.c b/apps/attitude_estimator_ekf/codegen/power.c new file mode 100755 index 0000000000..8672c7a85c --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/power.c @@ -0,0 +1,84 @@ +/* + * power.c + * + * Code generation for function 'power' + * + * C source code generated on: Mon Oct 01 19:38:49 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "power.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 f0; + real32_T f1; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else { + f0 = (real32_T)fabsf(u0); + f1 = (real32_T)fabsf(u1); + if (rtIsInfF(u1)) { + if (f0 == 1.0F) { + y = ((real32_T)rtNaN); + } else if (f0 > 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 (f1 == 0.0F) { + y = 1.0F; + } else if (f1 == 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)sqrtf(u0); + } else if ((u0 < 0.0F) && (u1 > (real32_T)floorf(u1))) { + y = ((real32_T)rtNaN); + } else { + y = (real32_T)powf(u0, u1); + } + } + + return y; +} + +/* + * + */ +void power(const real32_T a[12], real_T b, real32_T y[12]) +{ + int32_T k; + for (k = 0; k < 12; k++) { + y[k] = rt_powf_snf(a[k], (real32_T)b); + } +} + +/* End of code generation (power.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/find.h b/apps/attitude_estimator_ekf/codegen/power.h similarity index 55% rename from apps/attitude_estimator_ekf/codegen/find.h rename to apps/attitude_estimator_ekf/codegen/power.h index ca525c6009..a60f1bb25f 100755 --- a/apps/attitude_estimator_ekf/codegen/find.h +++ b/apps/attitude_estimator_ekf/codegen/power.h @@ -1,14 +1,14 @@ /* - * find.h + * power.h * - * Code generation for function 'find' + * Code generation for function 'power' * - * C source code generated on: Fri Sep 21 13:56:43 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ -#ifndef __FIND_H__ -#define __FIND_H__ +#ifndef __POWER_H__ +#define __POWER_H__ /* Include files */ #include #include @@ -29,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]); +extern void power(const real32_T a[12], real_T b, real32_T y[12]); #endif -/* End of code generation (find.h) */ +/* End of code generation (power.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index f1bb71c873..53686acc98 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:45 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index 6d32d51b56..5ac1dc7943 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:45 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index 50581f34df..1e1876b80f 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:45 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index 12d8734f5b..5f1c81f676 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h index 419edf01c9..5f65f6de98 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_defines.h +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index 42ff21d39e..2c687d7a1f 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index 60128156ea..d2ebd0806e 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index 2709915e95..b487c8b384 100755 --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Fri Sep 21 13:56:44 2012 + * C source code generated on: Mon Oct 01 19:38:49 2012 * */