diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index cad20d3753..b4d62ed145 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -44,7 +44,10 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rt_nonfinite.c \ codegen/rtGetInf.c \ codegen/rtGetNaN.c \ - codegen/norm.c + codegen/norm.c \ + codegen/find.c \ + codegen/diag.c \ + codegen/cross.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 09cbdd4e94..6c18b044e5 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -80,21 +80,35 @@ static float dt = 1; /* 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[12] = {0}; /**< */ -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, 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 +static float x_aposteriori_k[9] = {0}; /**< */ +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, + 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, + }; +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, + 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, }; /**< init: diagonal matrix with big values */ -static float knownConst[7] = {1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ +static float knownConst[20] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 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 @@ -203,13 +217,31 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) /* * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ + + //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]); + + int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; + float euler[3]; + int32_t z_k_sizes = 9; + float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + uint64_t timing_start = hrt_absolute_time(); - attitudeKalmanfilter(dt, z_k, x_aposteriori, P_aposteriori, knownConst, 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); + /* 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 % 200 == 0) { 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", euler[0], euler[1], 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 4e275e3ee9..321e6874d0 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:22 2012 * */ @@ -11,8 +11,11 @@ #include "rt_nonfinite.h" #include "attitudeKalmanfilter.h" #include "norm.h" +#include "cross.h" #include "eye.h" #include "mrdivide.h" +#include "diag.h" +#include "find.h" /* Type Definitions */ @@ -23,613 +26,691 @@ /* Variable Definitions */ /* Function Declarations */ +static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); /* Function Definitions */ +static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) +{ + real32_T y; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else if (rtIsInfF(u0) && rtIsInfF(u1)) { + y = (real32_T)atan2(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F); + } else if (u1 == 0.0F) { + if (u0 > 0.0F) { + y = RT_PIF / 2.0F; + } else if (u0 < 0.0F) { + y = -(RT_PIF / 2.0F); + } else { + y = 0.0F; + } + } else { + y = (real32_T)atan2(u0, u1); + } + + return y; +} /* - * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,updVect,z_k,u,x_aposteriori_k,P_aposteriori_k,knownConst) */ -void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T - x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T - knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T - P_aposteriori[144]) +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]) { + int32_T udpIndVect_sizes; + real_T udpIndVect_data[9]; real32_T R_temp[9]; real_T dv0[9]; - real_T dv1[9]; - int32_T i; + int32_T ib; int32_T i0; - real32_T A_pred[144]; - real32_T x_apriori[12]; - real32_T b_A_pred[144]; + 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 x_n_b[3]; + real32_T K_k_data[81]; int32_T i1; - real32_T c_A_pred[144]; + real32_T b_A_lin[81]; static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - real32_T P_apriori[144]; - real32_T b_P_apriori[108]; - static const int8_T iv1[108] = { 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, 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 }; - - real32_T K_k[108]; - static const int8_T iv2[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, 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, 1, 0, 0, 0, 0, 0, + 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, + 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 }; - real32_T fv0[81]; - real32_T fv1[81]; - real32_T fv2[81]; - real32_T B; - real_T dv2[144]; - real32_T b_B; - real32_T earth_z[3]; - real32_T y[3]; - real32_T earth_x[3]; + 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]; /* 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] */ /* */ /* [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 */ - /* 'attitudeKalmanfilter:19' acc_ProcessNoise=knownConst(1); */ - /* 'attitudeKalmanfilter:20' mag_ProcessNoise=knownConst(2); */ - /* 'attitudeKalmanfilter:21' ratesOffset_ProcessNoise=knownConst(3); */ - /* 'attitudeKalmanfilter:22' rates_ProcessNoise=knownConst(4); */ - /* 'attitudeKalmanfilter:25' acc_MeasurementNoise=knownConst(5); */ - /* 'attitudeKalmanfilter:26' mag_MeasurementNoise=knownConst(6); */ - /* 'attitudeKalmanfilter:27' rates_MeasurementNoise=knownConst(7); */ + /* 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); + + /* '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:30' Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:31' zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:32' zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3); */ - /* 'attitudeKalmanfilter:33' zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise]; */ - /* measurement noise covariance matrix */ - /* 'attitudeKalmanfilter:36' R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:37' zeros(3), eye(3)*mag_MeasurementNoise, zeros(3); */ - /* 'attitudeKalmanfilter:38' zeros(3), zeros(3), eye(3)*rates_MeasurementNoise]; */ + /* '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:42' H_k=[ eye(3), zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:43' zeros(3), eye(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:44' zeros(3), zeros(3), eye(3), eye(3)]; */ + /* '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:53' R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:54' dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:55' -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),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[11]; - R_temp[6] = dt * x_aposteriori_k[10]; - R_temp[1] = dt * x_aposteriori_k[11]; + 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[9]; - R_temp[2] = -dt * x_aposteriori_k[10]; - R_temp[5] = dt * x_aposteriori_k[9]; + 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; - /* strange, should not be transposed */ - /* 'attitudeKalmanfilter:58' A_pred=[R_temp', zeros(3), zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:59' zeros(3), R_temp', zeros(3), zeros(3); */ - /* 'attitudeKalmanfilter:60' zeros(3), zeros(3), eye(3), zeros(3); */ - /* 'attitudeKalmanfilter:61' zeros(3), zeros(3), zeros(3), eye(3)]; */ - eye(dv0); - eye(dv1); - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * i] = R_temp[i + 3 * i0]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 3)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 6)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 9)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i]; - } - } - + /* '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:64' x_apriori=A_pred*x_aposteriori_k; */ - for (i = 0; i < 12; i++) { - x_apriori[i] = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - x_apriori[i] += A_pred[i + 12 * i0] * x_aposteriori_k[i0]; + /* 'attitudeKalmanfilter:121' x_apriori=A_pred*x_aposteriori_k+B_pred*u(1:3); */ + 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 (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:67' acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:68' -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:69' dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0]; */ - /* 'attitudeKalmanfilter:71' mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:72' -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:73' dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0]; */ - /* 'attitudeKalmanfilter:75' A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat'; */ - /* 'attitudeKalmanfilter:76' zeros(3), R_temp', zeros(3), mag_temp_mat'; */ - /* 'attitudeKalmanfilter:77' zeros(3), zeros(3), eye(3), zeros(3); */ - /* 'attitudeKalmanfilter:78' zeros(3), zeros(3), zeros(3), eye(3)]; */ + /* '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)]; */ eye(dv0); eye(dv1); - for (i = 0; i < 3; i++) { + eye(dv2); + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * i] = R_temp[i + 3 * i0]; + A_lin[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 3)] = 0.0F; + A_lin[i0 + 9 * (ib + 3)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[i0 + 12 * (i + 6)] = 0.0F; + A_lin[i0 + 9 * (ib + 6)] = 0.0F; } } - A_pred[108] = 0.0F; - A_pred[109] = dt * x_aposteriori_k[2]; - A_pred[110] = -dt * x_aposteriori_k[1]; - A_pred[120] = -dt * x_aposteriori_k[2]; - A_pred[121] = 0.0F; - A_pred[122] = dt * x_aposteriori_k[0]; - A_pred[132] = dt * x_aposteriori_k[1]; - A_pred[133] = -dt * x_aposteriori_k[0]; - A_pred[134] = 0.0F; - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 3] = 0.0F; + A_lin[(i0 + 9 * ib) + 3] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0]; + A_lin[(i0 + 9 * (ib + 3)) + 3] = (real32_T)dv1[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; + A_lin[(i0 + 9 * (ib + 6)) + 3] = 0.0F; } } - A_pred[111] = 0.0F; - A_pred[112] = dt * x_aposteriori_k[5]; - A_pred[113] = -dt * x_aposteriori_k[4]; - A_pred[123] = -dt * x_aposteriori_k[5]; - A_pred[124] = 0.0F; - A_pred[125] = dt * x_aposteriori_k[3]; - A_pred[135] = dt * x_aposteriori_k[4]; - A_pred[136] = -dt * x_aposteriori_k[3]; - A_pred[137] = 0.0F; - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 6] = 0.0F; + A_lin[(i0 + 9 * ib) + 6] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; + A_lin[(i0 + 9 * (ib + 3)) + 6] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i]; + A_lin[(i0 + 9 * (ib + 6)) + 6] = (real32_T)dv2[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i]; - } - } - - /* 'attitudeKalmanfilter:81' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - b_A_pred[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_A_pred[i + 12 * i0] += A_pred[i + 12 * i1] * P_aposteriori_k[i1 + 12 * + /* '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 * i0]; } } - for (i0 = 0; i0 < 12; i0++) { - c_A_pred[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_A_pred[i + 12 * i0] += b_A_pred[i + 12 * i1] * A_pred[i0 + 12 * i1]; + 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 < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[0]; + K_k_data[i0 + 9 * ib] = R_temp[i0 + 3 * ib]; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 3)] = 0.0F; + K_k_data[i0 + 9 * (ib + 3)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 6)] = 0.0F; + K_k_data[i0 + 9 * (ib + 6)] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[i0 + 12 * (i + 9)] = 0.0F; + K_k_data[(i0 + 9 * ib) + 3] = 0.0F; } } - for (i = 0; i < 3; i++) { + for (ib = 0; ib < 3; ib++) { for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * - knownConst[1]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * - knownConst[2]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * i) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)iv0[i0 + 3 * i] * + K_k_data[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[3]; } } - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_apriori[i0 + 12 * i] = c_A_pred[i0 + 12 * i] + b_A_pred[i0 + 12 * i]; + for (ib = 0; ib < 3; ib++) { + for (i0 = 0; i0 < 3; i0++) { + K_k_data[(i0 + 9 * (ib + 6)) + 3] = 0.0F; } } - /* %update step */ - /* 'attitudeKalmanfilter:86' y_k=z_k-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:87' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:88' K_k=(P_apriori*H_k'/(S_k)); */ - for (i = 0; i < 12; i++) { + 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++) { - 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)iv1[i1 + - 12 * i0]; - } + P_apriori[i0 + 9 * ib] = b_A_lin[i0 + 9 * ib] + K_k_data[i0 + 9 * ib]; } } - 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)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; + /* '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]; } } - for (i0 = 0; i0 < 9; i0++) { - fv0[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; - } - } - } + /* %update step */ + /* 'attitudeKalmanfilter:140' accUpt=1; */ + accUpt = 1; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[4]; - } - } + /* 'attitudeKalmanfilter:141' magUpt=1; */ + magUpt = 1; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * (i + 3)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[i0 + 9 * (i + 6)] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * i) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * knownConst[5]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 6)) + 3] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * i) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - fv1[(i0 + 9 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * knownConst[6]; - } - } - - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 9; i0++) { - fv2[i0 + 9 * i] = fv0[i0 + 9 * i] + fv1[i0 + 9 * i]; - } - } - - mrdivide(b_P_apriori, fv2, K_k); - - /* 'attitudeKalmanfilter:91' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 9; i++) { - B = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - B += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; - } - - R_temp[i] = z_k[i] - B; - } - - for (i = 0; i < 12; i++) { - B = 0.0F; - for (i0 = 0; i0 < 9; i0++) { - B += K_k[i + 12 * i0] * R_temp[i0]; - } - - x_aposteriori[i] = x_apriori[i] + B; - } - - /* 'attitudeKalmanfilter:92' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ - b_eye(dv2); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - B = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - B += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; + /* 'attitudeKalmanfilter:142' y_k=z_k-H_k*x_apriori; */ + ia = udpIndVect_sizes - 1; + for (ib = 0; ib <= ia; ib++) { + y = 0.0F; + for (i0 = 0; i0 < 9; i0++) { + y += (real32_T)H_k_data[ib + udpIndVect_sizes * i0] * x_aposteriori[i0]; } - A_pred[i + 12 * i0] = (real32_T)dv2[i + 12 * i0] - B; + y_k_data[ib] = z_k_data[ib] - 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] += A_pred[i + 12 * i1] * P_apriori[i1 + 12 * - i0]; + /* '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)fabs(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++) { + 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]; + } + + A_lin[ib + H_k_sizes[0] * i0] = y + fv0[((int32_T)udpIndVect_data[ib] + + 9 * ((int32_T)udpIndVect_data[i0] - 1)) - 1]; + } + } + + 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]; + } + } + } + } 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)); } - /* %Rotation matrix generation */ - /* 'attitudeKalmanfilter:97' earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); */ - B = norm(*(real32_T (*)[3])&x_aposteriori[0]); + /* %% 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]); - /* 'attitudeKalmanfilter:98' earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); */ - b_B = norm(*(real32_T (*)[3])&x_aposteriori[3]); - for (i = 0; i < 3; i++) { - earth_z[i] = x_aposteriori[i] / B; - y[i] = x_aposteriori[i + 3] / b_B; + /* 'attitudeKalmanfilter:176' m_n_b = x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ + c_y = norm(*(real32_T (*)[3])&x_aposteriori[6]); + + /* '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; } - earth_x[0] = earth_z[1] * y[2] - earth_z[2] * y[1]; - earth_x[1] = earth_z[2] * y[0] - earth_z[0] * y[2]; - earth_x[2] = earth_z[0] * y[1] - earth_z[1] * y[0]; + cross(z_n_b, x_n_b, y_n_b); - /* 'attitudeKalmanfilter:99' earth_y=cross(earth_x,earth_z); */ - /* 'attitudeKalmanfilter:101' Rot_matrix=[earth_x,earth_y,earth_z]; */ - y[0] = earth_x[1] * earth_z[2] - earth_x[2] * earth_z[1]; - y[1] = earth_x[2] * earth_z[0] - earth_x[0] * earth_z[2]; - y[2] = earth_x[0] * earth_z[1] - earth_x[1] * earth_z[0]; - for (i = 0; i < 3; i++) { - Rot_matrix[i] = earth_x[i]; - Rot_matrix[3 + i] = y[i]; - Rot_matrix[6 + i] = earth_z[i]; + /* 'attitudeKalmanfilter:179' 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; } + + /* 'attitudeKalmanfilter:181' 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); */ + 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]; + } + + /* '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]; */ + eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); + eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); } /* End of code generation (attitudeKalmanfilter.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index 7aa3d048b7..8207aa5c53 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:22 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); +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]); #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 2800191c32..abf1519a67 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index d2d97bb3b9..20186f183c 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp deleted file mode 100755 index e69de29bb2..0000000000 diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat deleted file mode 100755 index 76fb78ca75..0000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat +++ /dev/null @@ -1,11 +0,0 @@ -@echo off -call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64 - -cd . -nmake -f attitudeKalmanfilter_rtw.mk GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0 -@if errorlevel 1 goto error_exit -exit /B 0 - -:error_exit -echo The make command returned an error of %errorlevel% -An_error_occurred_during_the_call_to_make diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk deleted file mode 100755 index b2123704b1..0000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk +++ /dev/null @@ -1,328 +0,0 @@ -# Copyright 1994-2010 The MathWorks, Inc. -# -# File : xrt_vcx64.tmf $Revision: 1.1.6.1 $ -# -# Abstract: -# Code generation template makefile for building a Windows-based, -# stand-alone real-time version of MATLAB-code using generated C/C++ -# code and the -# Microsoft Visual C/C++ compiler version 8, 9, 10 for x64 -# -# Note that this template is automatically customized by the -# code generation build procedure to create ".mk" -# -# The following defines can be used to modify the behavior of the -# build: -# -# OPT_OPTS - Optimization option. See DEFAULT_OPT_OPTS in -# vctools.mak for default. -# OPTS - User specific options. -# CPP_OPTS - C++ compiler options. -# USER_SRCS - Additional user sources, such as files needed by -# S-functions. -# USER_INCLUDES - Additional include paths -# (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2") -# -# To enable debugging: -# set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc) -# modify tmf LDFLAGS to include /DEBUG -# - -#------------------------ Macros read by make_rtw ----------------------------- -# -# The following macros are read by the code generation build procedure: -# -# MAKECMD - This is the command used to invoke the make utility -# HOST - What platform this template makefile is targeted for -# (i.e. PC or UNIX) -# BUILD - Invoke make from the code generation build procedure -# (yes/no)? -# SYS_TARGET_FILE - Name of system target file. - -MAKECMD = nmake -HOST = PC -BUILD = yes -SYS_TARGET_FILE = ert.tlc -BUILD_SUCCESS = ^#^#^# Created -COMPILER_TOOL_CHAIN = vcx64 - -#---------------------- Tokens expanded by make_rtw --------------------------- -# -# The following tokens, when wrapped with "|>" and "<|" are expanded by the -# code generation build procedure. -# -# MODEL_NAME - Name of the Simulink block diagram -# MODEL_MODULES - Any additional generated source modules -# MAKEFILE_NAME - Name of makefile created from template makefile .mk -# MATLAB_ROOT - Path to where MATLAB is installed. -# MATLAB_BIN - Path to MATLAB executable. -# S_FUNCTIONS - List of S-functions. -# S_FUNCTIONS_LIB - List of S-functions libraries to link. -# SOLVER - Solver source file name -# NUMST - Number of sample times -# TID01EQ - yes (1) or no (0): Are sampling rates of continuous task -# (tid=0) and 1st discrete task equal. -# NCSTATES - Number of continuous states -# BUILDARGS - Options passed in at the command line. -# MULTITASKING - yes (1) or no (0): Is solver mode multitasking -# EXT_MODE - yes (1) or no (0): Build for external mode -# TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode -# testing. -# EXTMODE_TRANSPORT - Index of transport mechanism (e.g. tcpip, serial) for extmode -# EXTMODE_STATIC - yes (1) or no (0): Use static instead of dynamic mem alloc. -# EXTMODE_STATIC_SIZE - Size of static memory allocation buffer. - -MODEL = attitudeKalmanfilter -MODULES = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c -MAKEFILE = attitudeKalmanfilter_rtw.mk -MATLAB_ROOT = C:\Program Files\MATLAB\R2011a -ALT_MATLAB_ROOT = C:\PROGRA~1\MATLAB\R2011a -MATLAB_BIN = C:\Program Files\MATLAB\R2011a\bin -ALT_MATLAB_BIN = C:\PROGRA~1\MATLAB\R2011a\bin -S_FUNCTIONS = -S_FUNCTIONS_LIB = -SOLVER = -NUMST = 1 -TID01EQ = 0 -NCSTATES = 0 -BUILDARGS = GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0 -MULTITASKING = 0 -EXT_MODE = 0 -TMW_EXTMODE_TESTING = 0 -EXTMODE_TRANSPORT = 0 -EXTMODE_STATIC = 0 -EXTMODE_STATIC_SIZE = 1000000 - -MODELREFS = -SHARED_SRC = -SHARED_SRC_DIR = -SHARED_BIN_DIR = -SHARED_LIB = -TARGET_LANG_EXT = c -OPTIMIZATION_FLAGS = /O2 /Oy- -ADDITIONAL_LDFLAGS = - -#--------------------------- Model and reference models ----------------------- -MODELLIB = attitudeKalmanfilter.lib -MODELREF_LINK_LIBS = -MODELREF_LINK_RSPFILE = attitudeKalmanfilter_ref.rsp -MODELREF_INC_PATH = -RELATIVE_PATH_TO_ANCHOR = F:\CODEGE~1 -MODELREF_TARGET_TYPE = RTW - -!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)" -MATLAB_ROOT = $(ALT_MATLAB_ROOT) -!endif -!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)" -MATLAB_BIN = $(ALT_MATLAB_BIN) -!endif - -#--------------------------- Tool Specifications ------------------------------ - -CPU = AMD64 -!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak -APPVER = 5.02 - -PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl -#------------------------------ Include/Lib Path ------------------------------ - -MATLAB_INCLUDES = $(MATLAB_ROOT)\simulink\include -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src -MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common - -# Additional file include paths - -MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter -MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter - -INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH) - -!if "$(SHARED_SRC_DIR)" != "" -INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR) -!endif - -#------------------------ External mode --------------------------------------- -# Uncomment -DVERBOSE to have information printed to stdout -# To add a new transport layer, see the comments in -# /toolbox/simulink/simulink/extmode_transports.m -!if $(EXT_MODE) == 1 -EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE -!if $(EXTMODE_TRANSPORT) == 0 #tcpip -EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c -EXT_LIB = wsock32.lib -!endif -!if $(EXTMODE_TRANSPORT) == 1 #serial_win32 -EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c -EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c -EXT_LIB = -!endif -!if $(TMW_EXTMODE_TESTING) == 1 -EXT_SRC = $(EXT_SRC) ext_test.c -EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING -!endif -!if $(EXTMODE_STATIC) == 1 -EXT_SRC = $(EXT_SRC) mem_mgr.c -EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE) -!endif -!else -EXT_SRC = -EXT_CC_OPTS = -EXT_LIB = -!endif - -#------------------------ rtModel ---------------------------------------------- - -RTM_CC_OPTS = -DUSE_RTMODEL - -#----------------- Compiler and Linker Options -------------------------------- - -# Optimization Options -# Set OPT_OPTS=-Zi for debugging (may depend on compiler version) -OPT_OPTS = $(DEFAULT_OPT_OPTS) - -# General User Options -OPTS = - -!if "$(OPTIMIZATION_FLAGS)" != "" -CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS) -!else -CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) -!endif - -CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \ - -DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \ - -DMT=$(MULTITASKING) -DHAVESTDIO - -# Uncomment this line to move warning level to W4 -# cflags = $(cflags:W3=W4) -CFLAGS = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \ - $(cflags) $(cvarsmt) /wd4996 -CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \ - $(cflags) $(cvarsmt) /wd4996 /EHsc- -LDFLAGS = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS) - -# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib - -#----------------------------- Source Files ----------------------------------- - - -#Standalone executable -!if "$(MODELREF_TARGET_TYPE)" == "NONE" -PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe -REQ_SRCS = $(MODULES) $(EXT_SRC) - -#Model Reference Target -!else -PRODUCT = $(MODELLIB) -REQ_SRCS = $(MODULES) $(EXT_SRC) -!endif - -USER_SRCS = - -SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS) -OBJS_CPP_UPPER = $(SRCS:.CPP=.obj) -OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj) -OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj) -OBJS = $(OBJS_C_UPPER:.c=.obj) -SHARED_OBJS = $(SHARED_SRC:.c=.obj) -# ------------------------- Additional Libraries ------------------------------ - -LIBS = - - -LIBS = $(LIBS) - -# ---------------------------- Linker Script ---------------------------------- - -CMD_FILE = $(MODEL).lnk -GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl - -#--------------------------------- Rules -------------------------------------- -all: set_environment_variables $(PRODUCT) - -!if "$(MODELREF_TARGET_TYPE)" == "NONE" -#--- Stand-alone model --- -$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS) - @cmd /C "echo ### Linking ..." - $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS) - $(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@ - @del $(CMD_FILE) - @cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe" -!else -#--- Model reference code generation Target --- -$(PRODUCT) : $(OBJS) $(SHARED_LIB) - @cmd /C "echo ### Linking ..." - $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS) - $(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB) - @cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)" -!endif - -{$(MATLAB_ROOT)\rtw\c\src}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -# Additional sources - - - - - -# Put these rule last, otherwise nmake will check toolboxes first - -{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CPPFLAGS) $< - -.c.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CFLAGS) $< - -.cpp.obj : - @cmd /C "echo ### Compiling $<" - $(CC) $(CPPFLAGS) $< - -!if "$(SHARED_LIB)" != "" -$(SHARED_LIB) : $(SHARED_SRC) - @cmd /C "echo ### Creating $@" - @$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<< -$? -<< - @$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS) - @cmd /C "echo ### $@ Created" -!endif - - -set_environment_variables: - @set INCLUDE=$(INCLUDE) - @set LIB=$(LIB) - -# Libraries: - - - - - -#----------------------------- Dependencies ----------------------------------- - -$(OBJS) : $(MAKEFILE) rtw_proj.tmw diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c index eab8c7d6e0..458ddb9eb0 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index fafd866e42..a939eee200 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h index 05f4664cd9..599a168848 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:22 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/buildInfo.mat b/apps/attitude_estimator_ekf/codegen/buildInfo.mat deleted file mode 100755 index b811d00397..0000000000 Binary files a/apps/attitude_estimator_ekf/codegen/buildInfo.mat and /dev/null differ diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c new file mode 100755 index 0000000000..49f655ece8 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -0,0 +1,37 @@ +/* + * cross.c + * + * Code generation for function 'cross' + * + * C source code generated on: Mon Sep 17 20:13:23 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "cross.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]) +{ + c[0] = a[1] * b[2] - a[2] * b[1]; + c[1] = a[2] * b[0] - a[0] * b[2]; + c[2] = a[0] * b[1] - a[1] * b[0]; +} + +/* End of code generation (cross.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h new file mode 100755 index 0000000000..844d8138fd --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -0,0 +1,34 @@ +/* + * cross.h + * + * Code generation for function 'cross' + * + * C source code generated on: Mon Sep 17 20:13:23 2012 + * + */ + +#ifndef __CROSS_H__ +#define __CROSS_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]); +#endif +/* End of code generation (cross.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c new file mode 100755 index 0000000000..e54d52a384 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/diag.c @@ -0,0 +1,42 @@ +/* + * diag.c + * + * Code generation for function 'diag' + * + * C source code generated on: Mon Sep 17 20:13:22 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "diag.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void diag(const real32_T v[3], real32_T d[9]) +{ + int32_T j; + for (j = 0; j < 9; j++) { + d[j] = 0.0F; + } + + for (j = 0; j < 3; j++) { + d[j + 3 * 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 new file mode 100755 index 0000000000..a4a2a4c829 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -0,0 +1,34 @@ +/* + * diag.h + * + * Code generation for function 'diag' + * + * C source code generated on: Mon Sep 17 20:13:23 2012 + * + */ + +#ifndef __DIAG_H__ +#define __DIAG_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void diag(const real32_T v[3], real32_T d[9]); +#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 e4655839c4..aece401c25 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -27,12 +27,12 @@ /* * */ -void b_eye(real_T I[144]) +void b_eye(real_T I[81]) { int32_T i; - memset((void *)&I[0], 0, 144U * sizeof(real_T)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1.0; + memset((void *)&I[0], 0, 81U * sizeof(real_T)); + for (i = 0; i < 9; i++) { + I[i + 9 * i] = 1.0; } } diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h index e8881747f6..e715ae1c39 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,7 +29,7 @@ /* Variable Definitions */ /* Function Declarations */ -extern void b_eye(real_T I[144]); +extern void b_eye(real_T I[81]); 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 new file mode 100755 index 0000000000..532ba43970 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/find.c @@ -0,0 +1,77 @@ +/* + * find.c + * + * Code generation for function 'find' + * + * C source code generated on: Mon Sep 17 20:13:22 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/find.h b/apps/attitude_estimator_ekf/codegen/find.h new file mode 100755 index 0000000000..ceb90b6cf6 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/find.h @@ -0,0 +1,34 @@ +/* + * find.h + * + * Code generation for function 'find' + * + * C source code generated on: Mon Sep 17 20:13:22 2012 + * + */ + +#ifndef __FIND_H__ +#define __FIND_H__ +/* Include files */ +#include +#include +#include +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]); +#endif +/* End of code generation (find.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c index cb81b53282..2fcaf8cb67 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -21,136 +21,719 @@ /* 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 */ /* * */ -void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) +static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x) { - int32_T jy; + return 0.0F; +} + +/* + * + */ +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]) +{ + int32_T loop_ub; int32_T iy; - real32_T b_A[81]; - int8_T ipiv[9]; + real32_T b_A_data[81]; + int32_T jA; + int32_T tmp_data[9]; + int32_T ipiv_data[9]; + int32_T ldap1; 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 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); + 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]; } - for (j = 0; j < 8; j++) { - mmj = -j; - jj = j * 10; - jp1j = jj + 1; - c = mmj + 9; - jy = 0; - ix = jj; - temp = fabsf(b_A[jj]); - for (k = 2; k <= c; k++) { - ix++; - s = fabsf(b_A[ix]); - if (s > temp) { - jy = k - 1; - temp = s; + 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; + } + + loop_ub = jA - 1; + for (iy = 0; iy <= loop_ub; iy++) { + ipiv_data[iy] = tmp_data[iy]; + } + } + + 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; + } + } } } - 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; + 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]; } } - loop_ub = (jp1j + mmj) + 8; - for (iy = jp1j; iy + 1 <= loop_ub; iy++) { - b_A[iy] /= b_A[jj]; + b_loop_ub = jj + mmj; + for (jA = jj + 1; jA + 1 <= b_loop_ub; jA++) { + b_A_data[jA] /= b_A_data[jj]; } } - 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; + 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; ix++; } } - jy += 9; - jj += 9; + iy += A_sizes[1]; + jA += A_sizes[1]; } } - for (jy = 0; jy < 12; jy++) { - for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * jy] = A[jy + 12 * iy]; - } - } - - 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; + 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 (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]; + 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 = 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]; + 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]; + } + } + } + } + } +} + +/* + * + */ +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) +{ + 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 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); + } } } } } - for (jy = 0; jy < 9; jy++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * jy] = Y[jy + 9 * iy]; + 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; + 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; + } + } + } + + y = scale * (real32_T)sqrt(y); + } + + return y; +} + +/* + * + */ +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]; + } + } + + 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]; + } + } + + 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; + } + } 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]; } } } diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h index e81bfffc0a..a58faaa262 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" @@ -27,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); +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]); #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 1fbde052bc..8d8e4e1109 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -40,7 +40,7 @@ real32_T norm(const real32_T x[3]) firstNonZero = TRUE; for (k = 0; k < 3; k++) { if (x[k] != 0.0F) { - absxk = fabsf(x[k]); + absxk = (real32_T)fabs(x[k]); if (firstNonZero) { scale = absxk; y = 1.0F; @@ -56,7 +56,7 @@ real32_T norm(const real32_T x[3]) } } - return scale * sqrtf(y); + return scale * (real32_T)sqrt(y); } /* End of code generation (norm.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h index b0c7fe430e..479503ae36 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:23 2012 * */ @@ -14,6 +14,8 @@ #include #include #include +#include "rt_defines.h" +#include "rt_nonfinite.h" #include "rtwtypes.h" #include "attitudeKalmanfilter_types.h" diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index 3cef176848..42b3af501f 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index ab2d5a70d5..20d8c7f058 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index 17a093461f..d29aea34b6 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index 2c254bbbf6..a14b6170b8 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h new file mode 100755 index 0000000000..4cad63ada3 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -0,0 +1,24 @@ +/* + * rt_defines.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Mon Sep 17 20:13:24 2012 + * + */ + +#ifndef __RT_DEFINES_H__ +#define __RT_DEFINES_H__ + +#include + +#define RT_PI 3.14159265358979323846 +#define RT_PIF 3.1415927F +#define RT_LN_10 2.30258509299404568402 +#define RT_LN_10F 2.3025851F +#define RT_LOG10E 0.43429448190325182765 +#define RT_LOG10EF 0.43429449F +#define RT_E 2.7182818284590452354 +#define RT_EF 2.7182817F +#endif +/* End of code generation (rt_defines.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index 005c4f28de..bd04f52e66 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index 6481f011f8..67b3ba205d 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw b/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw deleted file mode 100755 index 1fb585abd7..0000000000 --- a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw +++ /dev/null @@ -1 +0,0 @@ -Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a. diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index fd629ebcd0..6feb2e1a99 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: Wed Jul 11 08:38:35 2012 + * C source code generated on: Mon Sep 17 20:13:24 2012 * */