From ac215185a9604a88665d0b5b8382659f8cedaf56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jan 2013 17:30:53 +0100 Subject: Better attitude filter, not sensitive to sudden accelerations --- apps/attitude_estimator_ekf/Makefile | 2 - .../attitude_estimator_ekf_main.c | 4 + .../attitude_estimator_ekf_params.c | 40 +- .../attitude_estimator_ekf_params.h | 4 +- .../codegen/attitudeKalmanfilter.c | 876 +++++++++++++++------ .../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/eye.c | 2 +- apps/attitude_estimator_ekf/codegen/eye.h | 2 +- apps/attitude_estimator_ekf/codegen/mrdivide.c | 2 +- apps/attitude_estimator_ekf/codegen/mrdivide.h | 2 +- apps/attitude_estimator_ekf/codegen/norm.c | 2 +- apps/attitude_estimator_ekf/codegen/norm.h | 2 +- apps/attitude_estimator_ekf/codegen/rdivide.c | 2 +- apps/attitude_estimator_ekf/codegen/rdivide.h | 2 +- apps/attitude_estimator_ekf/codegen/rtGetInf.c | 2 +- apps/attitude_estimator_ekf/codegen/rtGetInf.h | 2 +- apps/attitude_estimator_ekf/codegen/rtGetNaN.c | 2 +- apps/attitude_estimator_ekf/codegen/rtGetNaN.h | 2 +- apps/attitude_estimator_ekf/codegen/rt_defines.h | 2 +- apps/attitude_estimator_ekf/codegen/rt_nonfinite.c | 2 +- apps/attitude_estimator_ekf/codegen/rt_nonfinite.h | 2 +- apps/attitude_estimator_ekf/codegen/rtwtypes.h | 6 +- 29 files changed, 652 insertions(+), 328 deletions(-) mode change 100644 => 100755 apps/attitude_estimator_ekf/Makefile mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/cross.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/cross.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/eye.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/eye.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/mrdivide.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/mrdivide.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/norm.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/norm.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rdivide.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rdivide.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rtGetInf.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rtGetInf.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rtGetNaN.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rtGetNaN.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rt_defines.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rt_nonfinite.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rt_nonfinite.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rtwtypes.h (limited to 'apps/attitude_estimator_ekf') diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile old mode 100644 new mode 100755 index 4d531867c..734af7cc1 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -47,8 +47,6 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rtGetInf.c \ codegen/rtGetNaN.c \ codegen/norm.c \ - codegen/diag.c \ - codegen/power.c \ codegen/cross.c diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c old mode 100644 new mode 100755 index 1c3b9976b..7ca77e513 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -431,6 +431,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; att.yawspeed = x_aposteriori[2]; + att.rollacc = x_aposteriori[3]; + att.pitchacc = x_aposteriori[4]; + att.yawacc = x_aposteriori[5]; + //att.yawspeed =z_k[2] ; /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c old mode 100644 new mode 100755 index 91fec9ccb..5b7756611 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -50,15 +50,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_Q2, 1e1f); /* gyro offsets process noise */ PARAM_DEFINE_FLOAT(EKF_ATT_Q3, 1e-4f); PARAM_DEFINE_FLOAT(EKF_ATT_Q4, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q5, 1e-4f); -/* accelerometer process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_Q6, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q7, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q8, 1e-1f); -/* magnetometer process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_Q9, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f); /* gyro measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f); @@ -66,12 +57,7 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f); PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f); /* accelerometer measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f); -PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f); -PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f); -/* magnetometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f); + /* offsets in roll, pitch and yaw of sensor plane and body */ PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); @@ -85,23 +71,11 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->q2 = param_find("EKF_ATT_Q2"); h->q3 = param_find("EKF_ATT_Q3"); h->q4 = param_find("EKF_ATT_Q4"); - h->q5 = param_find("EKF_ATT_Q5"); - h->q6 = param_find("EKF_ATT_Q6"); - h->q7 = param_find("EKF_ATT_Q7"); - h->q8 = param_find("EKF_ATT_Q8"); - h->q9 = param_find("EKF_ATT_Q9"); - h->q10 = param_find("EKF_ATT_Q10"); - h->q11 = param_find("EKF_ATT_Q11"); h->r0 = param_find("EKF_ATT_R0"); h->r1 = param_find("EKF_ATT_R1"); h->r2 = param_find("EKF_ATT_R2"); h->r3 = param_find("EKF_ATT_R3"); - h->r4 = param_find("EKF_ATT_R4"); - h->r5 = param_find("EKF_ATT_R5"); - h->r6 = param_find("EKF_ATT_R6"); - h->r7 = param_find("EKF_ATT_R7"); - h->r8 = param_find("EKF_ATT_R8"); h->roll_off = param_find("ATT_ROLL_OFFS"); h->pitch_off = param_find("ATT_PITCH_OFFS"); @@ -117,23 +91,11 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->q2, &(p->q[2])); param_get(h->q3, &(p->q[3])); param_get(h->q4, &(p->q[4])); - param_get(h->q5, &(p->q[5])); - param_get(h->q6, &(p->q[6])); - param_get(h->q7, &(p->q[7])); - param_get(h->q8, &(p->q[8])); - param_get(h->q9, &(p->q[9])); - param_get(h->q10, &(p->q[10])); - param_get(h->q11, &(p->q[11])); param_get(h->r0, &(p->r[0])); param_get(h->r1, &(p->r[1])); param_get(h->r2, &(p->r[2])); param_get(h->r3, &(p->r[3])); - param_get(h->r4, &(p->r[4])); - param_get(h->r5, &(p->r[5])); - param_get(h->r6, &(p->r[6])); - param_get(h->r7, &(p->r[7])); - param_get(h->r8, &(p->r[8])); param_get(h->roll_off, &(p->roll_off)); param_get(h->pitch_off, &(p->pitch_off)); diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h old mode 100644 new mode 100755 index b315d5fe8..09817d58e --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -50,8 +50,8 @@ struct attitude_estimator_ekf_params { }; struct attitude_estimator_ekf_param_handles { - param_t r0, r1, r2, r3, r4, r5, r6, r7, r8; - param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11; + param_t r0, r1, r2, r3; + param_t q0, q1, q2, q3, q4; param_t roll_off, pitch_off, yaw_off; }; diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c old mode 100644 new mode 100755 index 04f6ea267..9e97ad11a --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ @@ -15,8 +15,6 @@ #include "cross.h" #include "eye.h" #include "mrdivide.h" -#include "diag.h" -#include "power.h" /* Type Definitions */ @@ -50,7 +48,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) b_u1 = -1; } - y = (real32_T)atan2f((real32_T)b_u0, (real32_T)b_u1); + y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1); } else if (u1 == 0.0F) { if (u0 > 0.0F) { y = RT_PIF / 2.0F; @@ -60,7 +58,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) y = 0.0F; } } else { - y = (real32_T)atan2f(u0, u1); + y = (real32_T)atan2(u0, u1); } return y; @@ -71,80 +69,88 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) */ 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 + P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]) { - real32_T a[12]; - real32_T b_a[12]; - int32_T i; - real32_T Q[144]; + real32_T wak[3]; real32_T O[9]; real_T dv0[9]; - real32_T c_a[9]; - real32_T d_a[9]; + real32_T a[9]; + int32_T i; + real32_T b_a[9]; real32_T x_n_b[3]; real32_T b_x_aposteriori_k[3]; - real32_T m_n_b[3]; real32_T z_n_b[3]; - real32_T x_apriori[12]; + real32_T c_a[3]; + real32_T d_a[3]; int32_T i0; + real32_T x_apriori[12]; real_T dv1[144]; real32_T A_lin[144]; + static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 b_A_lin[144]; + real32_T b_q[144]; + real32_T c_A_lin[144]; + real32_T d_A_lin[144]; + real32_T e_A_lin[144]; int32_T i1; real32_T P_apriori[144]; - real32_T f0; - real32_T R[81]; real32_T b_P_apriori[108]; - static const int8_T iv0[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 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, 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 K_k[108]; - static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + real32_T fv0[81]; + 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, 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, 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 b_r[81]; + real32_T fv1[81]; + real32_T f0; real32_T c_P_apriori[36]; - static const int8_T iv2[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + static const int8_T iv3[36] = { 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 }; - real32_T fv1[36]; - static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + real32_T fv2[36]; + static const int8_T iv4[36] = { 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, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - real32_T S_k[36]; + real32_T c_r[9]; + real32_T b_K_k[36]; real32_T d_P_apriori[72]; - static const int8_T iv4[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + static const int8_T iv5[72] = { 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, 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, + real32_T c_K_k[72]; + static const int8_T iv6[72] = { 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, 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, 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, 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 }; - + real32_T b_z[6]; static const int8_T iv7[72] = { 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, 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]; + static const int8_T iv8[72] = { 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, 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 }; + + real32_T fv3[6]; + real32_T c_z[6]; /* Extended Attitude Kalmanfilter */ /* */ @@ -160,44 +166,31 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const /* 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, a); - for (i = 0; i < 12; i++) { - b_a[i] = a[i] * dt; - } - - diag(b_a, Q); - - /* Q=[1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, 0, 0, 1e-10, 0, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ - /* 0, 0, 0, 0, 1e-10, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ - /* 0, 0, 0, 0, 0, 1e-10, 0, 0, 0, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1]; */ + /* Q = diag(q.^2*dt); */ /* observation matrix */ - /* 'attitudeKalmanfilter:44' wx= x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:45' wy= x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:46' wz= x_aposteriori_k(3); */ - /* 'attitudeKalmanfilter:48' wox= x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:49' woy= x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:50' woz= x_aposteriori_k(6); */ - /* 'attitudeKalmanfilter:52' zex= x_aposteriori_k(7); */ - /* 'attitudeKalmanfilter:53' zey= x_aposteriori_k(8); */ - /* 'attitudeKalmanfilter:54' zez= x_aposteriori_k(9); */ - /* 'attitudeKalmanfilter:56' mux= x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:57' muy= x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:58' muz= x_aposteriori_k(12); */ - /* 'attitudeKalmanfilter:61' wk =[wx; */ - /* 'attitudeKalmanfilter:62' wy; */ - /* 'attitudeKalmanfilter:63' wz]; */ - /* 'attitudeKalmanfilter:65' wok =[wox;woy;woz]; */ - /* 'attitudeKalmanfilter:66' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */ + /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */ + /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */ + /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */ + /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */ + /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */ + /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */ + /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */ + /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */ + /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */ + /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */ + /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */ + /* % prediction section */ + /* body angular accelerations */ + /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */ + wak[0] = x_aposteriori_k[3]; + wak[1] = x_aposteriori_k[4]; + wak[2] = x_aposteriori_k[5]; + + /* body angular rates */ + /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */ + /* derivative of the prediction rotation matrix */ + /* 'attitudeKalmanfilter:57' 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]; @@ -208,69 +201,73 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const O[7] = x_aposteriori_k[0]; O[8] = 0.0F; - /* 'attitudeKalmanfilter:67' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ + /* prediction of the earth z vector */ + /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ eye(dv0); for (i = 0; i < 9; i++) { - c_a[i] = (real32_T)dv0[i] + O[i] * dt; + a[i] = (real32_T)dv0[i] + O[i] * dt; } - /* 'attitudeKalmanfilter:68' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ + /* prediction of the magnetic vector */ + /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ eye(dv0); for (i = 0; i < 9; i++) { - d_a[i] = (real32_T)dv0[i] + O[i] * dt; + b_a[i] = (real32_T)dv0[i] + O[i] * dt; } - /* 'attitudeKalmanfilter:70' EZ=[0,zez,-zey; */ - /* 'attitudeKalmanfilter:71' -zez,0,zex; */ - /* 'attitudeKalmanfilter:72' zey,-zex,0]'; */ - /* 'attitudeKalmanfilter:73' MA=[0,muz,-muy; */ - /* 'attitudeKalmanfilter:74' -muz,0,mux; */ - /* 'attitudeKalmanfilter:75' zey,-mux,0]'; */ - /* 'attitudeKalmanfilter:79' E=eye(3); */ - /* 'attitudeKalmanfilter:80' Es=[1,0,0; */ - /* 'attitudeKalmanfilter:81' 0,1,0; */ - /* 'attitudeKalmanfilter:82' 0,0,0]; */ - /* 'attitudeKalmanfilter:83' Z=zeros(3); */ - /* 'attitudeKalmanfilter:84' 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]; - b_x_aposteriori_k[0] = x_aposteriori_k[9]; - b_x_aposteriori_k[1] = x_aposteriori_k[10]; - b_x_aposteriori_k[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]; + /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */ + /* 'attitudeKalmanfilter:66' -zez,0,zex; */ + /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */ + /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */ + /* 'attitudeKalmanfilter:69' -muz,0,mux; */ + /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */ + /* 'attitudeKalmanfilter:74' E=eye(3); */ + /* 'attitudeKalmanfilter:76' Z=zeros(3); */ + /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */ + x_n_b[0] = x_aposteriori_k[0]; + x_n_b[1] = x_aposteriori_k[1]; + x_n_b[2] = x_aposteriori_k[2]; + b_x_aposteriori_k[0] = x_aposteriori_k[6]; + b_x_aposteriori_k[1] = x_aposteriori_k[7]; + b_x_aposteriori_k[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]; for (i = 0; i < 3; i++) { - m_n_b[i] = 0.0F; + c_a[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - m_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0]; + c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; } - z_n_b[i] = 0.0F; + d_a[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - z_n_b[i] += d_a[i + 3 * i0] * b_x_aposteriori_k[i0]; + d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; } - x_apriori[i + 6] = m_n_b[i]; + x_apriori[i] = x_n_b[i] + dt * wak[i]; + } + + for (i = 0; i < 3; i++) { + x_apriori[i + 3] = wak[i]; + } + + for (i = 0; i < 3; i++) { + x_apriori[i + 6] = c_a[i]; } for (i = 0; i < 3; i++) { - x_apriori[i + 9] = z_n_b[i]; + x_apriori[i + 9] = d_a[i]; } - /* 'attitudeKalmanfilter:86' A_lin=[ Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:87' Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:88' EZ, Z, O, Z */ - /* 'attitudeKalmanfilter:89' MA, Z, Z, O]; */ - /* 'attitudeKalmanfilter:92' A_lin=eye(12)+A_lin*dt; */ + /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */ + /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */ + /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */ + /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */ + /* 'attitudeKalmanfilter:86' 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; + A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i]; } for (i0 = 0; i0 < 3; i0++) { @@ -339,7 +336,164 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } } - /* 'attitudeKalmanfilter:98' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */ + /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */ + /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */ + /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */ + /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */ + /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + b_q[0] = q[0]; + b_q[12] = 0.0F; + b_q[24] = 0.0F; + b_q[36] = 0.0F; + b_q[48] = 0.0F; + b_q[60] = 0.0F; + b_q[72] = 0.0F; + b_q[84] = 0.0F; + b_q[96] = 0.0F; + b_q[108] = 0.0F; + b_q[120] = 0.0F; + b_q[132] = 0.0F; + b_q[1] = 0.0F; + b_q[13] = q[0]; + b_q[25] = 0.0F; + b_q[37] = 0.0F; + b_q[49] = 0.0F; + b_q[61] = 0.0F; + b_q[73] = 0.0F; + b_q[85] = 0.0F; + b_q[97] = 0.0F; + b_q[109] = 0.0F; + b_q[121] = 0.0F; + b_q[133] = 0.0F; + b_q[2] = 0.0F; + b_q[14] = 0.0F; + b_q[26] = q[0]; + b_q[38] = 0.0F; + b_q[50] = 0.0F; + b_q[62] = 0.0F; + b_q[74] = 0.0F; + b_q[86] = 0.0F; + b_q[98] = 0.0F; + b_q[110] = 0.0F; + b_q[122] = 0.0F; + b_q[134] = 0.0F; + b_q[3] = 0.0F; + b_q[15] = 0.0F; + b_q[27] = 0.0F; + b_q[39] = q[1]; + b_q[51] = 0.0F; + b_q[63] = 0.0F; + b_q[75] = 0.0F; + b_q[87] = 0.0F; + b_q[99] = 0.0F; + b_q[111] = 0.0F; + b_q[123] = 0.0F; + b_q[135] = 0.0F; + b_q[4] = 0.0F; + b_q[16] = 0.0F; + b_q[28] = 0.0F; + b_q[40] = 0.0F; + b_q[52] = q[1]; + b_q[64] = 0.0F; + b_q[76] = 0.0F; + b_q[88] = 0.0F; + b_q[100] = 0.0F; + b_q[112] = 0.0F; + b_q[124] = 0.0F; + b_q[136] = 0.0F; + b_q[5] = 0.0F; + b_q[17] = 0.0F; + b_q[29] = 0.0F; + b_q[41] = 0.0F; + b_q[53] = 0.0F; + b_q[65] = q[1]; + b_q[77] = 0.0F; + b_q[89] = 0.0F; + b_q[101] = 0.0F; + b_q[113] = 0.0F; + b_q[125] = 0.0F; + b_q[137] = 0.0F; + b_q[6] = 0.0F; + b_q[18] = 0.0F; + b_q[30] = 0.0F; + b_q[42] = 0.0F; + b_q[54] = 0.0F; + b_q[66] = 0.0F; + b_q[78] = q[2]; + b_q[90] = 0.0F; + b_q[102] = 0.0F; + b_q[114] = 0.0F; + b_q[126] = 0.0F; + b_q[138] = 0.0F; + b_q[7] = 0.0F; + b_q[19] = 0.0F; + b_q[31] = 0.0F; + b_q[43] = 0.0F; + b_q[55] = 0.0F; + b_q[67] = 0.0F; + b_q[79] = 0.0F; + b_q[91] = q[2]; + b_q[103] = 0.0F; + b_q[115] = 0.0F; + b_q[127] = 0.0F; + b_q[139] = 0.0F; + b_q[8] = 0.0F; + b_q[20] = 0.0F; + b_q[32] = 0.0F; + b_q[44] = 0.0F; + b_q[56] = 0.0F; + b_q[68] = 0.0F; + b_q[80] = 0.0F; + b_q[92] = 0.0F; + b_q[104] = q[2]; + b_q[116] = 0.0F; + b_q[128] = 0.0F; + b_q[140] = 0.0F; + b_q[9] = 0.0F; + b_q[21] = 0.0F; + b_q[33] = 0.0F; + b_q[45] = 0.0F; + b_q[57] = 0.0F; + b_q[69] = 0.0F; + b_q[81] = 0.0F; + b_q[93] = 0.0F; + b_q[105] = 0.0F; + b_q[117] = q[3]; + b_q[129] = 0.0F; + b_q[141] = 0.0F; + b_q[10] = 0.0F; + b_q[22] = 0.0F; + b_q[34] = 0.0F; + b_q[46] = 0.0F; + b_q[58] = 0.0F; + b_q[70] = 0.0F; + b_q[82] = 0.0F; + b_q[94] = 0.0F; + b_q[106] = 0.0F; + b_q[118] = 0.0F; + b_q[130] = q[3]; + b_q[142] = 0.0F; + b_q[11] = 0.0F; + b_q[23] = 0.0F; + b_q[35] = 0.0F; + b_q[47] = 0.0F; + b_q[59] = 0.0F; + b_q[71] = 0.0F; + b_q[83] = 0.0F; + b_q[95] = 0.0F; + b_q[107] = 0.0F; + b_q[119] = 0.0F; + b_q[131] = 0.0F; + b_q[143] = q[3]; for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { A_lin[i + 12 * i0] = 0.0F; @@ -347,38 +501,63 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * i0]; } + + c_A_lin[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0]; + } } - } - for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; + d_A_lin[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - f0 += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; + d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; } - P_apriori[i + 12 * i0] = f0 + Q[i + 12 * i0]; + e_A_lin[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; + } } } - /* %update */ - /* 'attitudeKalmanfilter:102' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i]; + } + } + + /* % update */ + /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:103' R=diag(r); */ - b_diag(r, R); + /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */ + if ((z[5] < 4.0F) || (z[4] > 15.0F)) { + /* 'attitudeKalmanfilter:112' r(2)=10000; */ + r[1] = 10000.0F; + } + /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */ + /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */ + /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */ + /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */ + /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */ + /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */ + /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */ + /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */ + /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:106' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:107' Z, Z, E, Z; */ - /* 'attitudeKalmanfilter:108' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:110' y_k=z(1:9)-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:112' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:113' K_k=(P_apriori*H_k'/(S_k)); */ + /* [zw;ze;zmk]; */ + /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */ + /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */ + /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */ + /* 'attitudeKalmanfilter:133' 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 + b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 + 12 * i0]; } } @@ -388,53 +567,136 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const 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]; + K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; } } - } - for (i = 0; i < 9; i++) { for (i0 = 0; i0 < 9; i0++) { - f0 = 0.0F; + fv0[i + 9 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - f0 += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0]; + fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; } + } + } - fv0[i + 9 * i0] = f0 + R[i + 9 * i0]; + b_r[0] = r[0]; + b_r[9] = 0.0F; + b_r[18] = 0.0F; + b_r[27] = 0.0F; + b_r[36] = 0.0F; + b_r[45] = 0.0F; + b_r[54] = 0.0F; + b_r[63] = 0.0F; + b_r[72] = 0.0F; + b_r[1] = 0.0F; + b_r[10] = r[0]; + b_r[19] = 0.0F; + b_r[28] = 0.0F; + b_r[37] = 0.0F; + b_r[46] = 0.0F; + b_r[55] = 0.0F; + b_r[64] = 0.0F; + b_r[73] = 0.0F; + b_r[2] = 0.0F; + b_r[11] = 0.0F; + b_r[20] = r[0]; + b_r[29] = 0.0F; + b_r[38] = 0.0F; + b_r[47] = 0.0F; + b_r[56] = 0.0F; + b_r[65] = 0.0F; + b_r[74] = 0.0F; + b_r[3] = 0.0F; + b_r[12] = 0.0F; + b_r[21] = 0.0F; + b_r[30] = r[1]; + b_r[39] = 0.0F; + b_r[48] = 0.0F; + b_r[57] = 0.0F; + b_r[66] = 0.0F; + b_r[75] = 0.0F; + b_r[4] = 0.0F; + b_r[13] = 0.0F; + b_r[22] = 0.0F; + b_r[31] = 0.0F; + b_r[40] = r[1]; + b_r[49] = 0.0F; + b_r[58] = 0.0F; + b_r[67] = 0.0F; + b_r[76] = 0.0F; + b_r[5] = 0.0F; + b_r[14] = 0.0F; + b_r[23] = 0.0F; + b_r[32] = 0.0F; + b_r[41] = 0.0F; + b_r[50] = r[1]; + b_r[59] = 0.0F; + b_r[68] = 0.0F; + b_r[77] = 0.0F; + b_r[6] = 0.0F; + b_r[15] = 0.0F; + b_r[24] = 0.0F; + b_r[33] = 0.0F; + b_r[42] = 0.0F; + b_r[51] = 0.0F; + b_r[60] = r[2]; + b_r[69] = 0.0F; + b_r[78] = 0.0F; + b_r[7] = 0.0F; + b_r[16] = 0.0F; + b_r[25] = 0.0F; + b_r[34] = 0.0F; + b_r[43] = 0.0F; + b_r[52] = 0.0F; + b_r[61] = 0.0F; + b_r[70] = r[2]; + b_r[79] = 0.0F; + b_r[8] = 0.0F; + b_r[17] = 0.0F; + b_r[26] = 0.0F; + b_r[35] = 0.0F; + b_r[44] = 0.0F; + b_r[53] = 0.0F; + b_r[62] = 0.0F; + b_r[71] = 0.0F; + b_r[80] = r[2]; + for (i = 0; i < 9; i++) { + for (i0 = 0; i0 < 9; i0++) { + fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i]; } } - mrdivide(b_P_apriori, fv0, K_k); + mrdivide(b_P_apriori, fv1, K_k); - /* 'attitudeKalmanfilter:116' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 9; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv1[i + 9 * i0] * x_apriori[i0]; + f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; } - c_a[i] = z[i] - f0; + O[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 9; i0++) { - f0 += K_k[i + 12 * i0] * c_a[i0]; + f0 += K_k[i + 12 * i0] * O[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:117' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ + /* 'attitudeKalmanfilter:137' 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++) { f0 = 0.0F; for (i1 = 0; i1 < 9; i1++) { - f0 += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0]; + f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -442,60 +704,72 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const 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]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 + * i0]; } } } } else { - /* 'attitudeKalmanfilter:118' else */ - /* 'attitudeKalmanfilter:119' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ + /* 'attitudeKalmanfilter:138' else */ + /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */ - c_diag(*(real32_T (*)[3])&r[0], O); - + /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */ + /* 'attitudeKalmanfilter:142' 0,r(1),0; */ + /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:123' H_k=[ E, Z, Z, Z]; */ - /* 'attitudeKalmanfilter:125' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:127' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'attitudeKalmanfilter:128' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */ + /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'attitudeKalmanfilter:151' 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]; + iv3[i1 + 12 * i0]; } } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 12; i0++) { - fv1[i + 3 * i0] = 0.0F; + fv2[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 * + fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 * i0]; } } - } - for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; + O[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - f0 += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; + O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0]; } + } + } - c_a[i + 3 * i0] = f0 + O[i + 3 * i0]; + c_r[0] = r[0]; + c_r[3] = 0.0F; + c_r[6] = 0.0F; + c_r[1] = 0.0F; + c_r[4] = r[0]; + c_r[7] = 0.0F; + c_r[2] = 0.0F; + c_r[5] = 0.0F; + c_r[8] = r[0]; + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i]; } } - b_mrdivide(c_P_apriori, c_a, S_k); + b_mrdivide(c_P_apriori, a, b_K_k); - /* 'attitudeKalmanfilter:131' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv3[i + 3 * i0] * x_apriori[i0]; + f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0]; } x_n_b[i] = z[i] - f0; @@ -504,22 +778,22 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 3; i0++) { - f0 += S_k[i + 12 * i0] * x_n_b[i0]; + f0 += b_K_k[i + 12 * i0] * x_n_b[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:132' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:155' 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++) { f0 = 0.0F; for (i1 = 0; i1 < 3; i1++) { - f0 += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0]; + f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -527,87 +801,134 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const 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]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + + 12 * i0]; } } } } else { - /* 'attitudeKalmanfilter:133' else */ - /* 'attitudeKalmanfilter:134' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ + /* 'attitudeKalmanfilter:156' else */ + /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */ - d_diag(*(real32_T (*)[6])&r[0], S_k); + /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */ + if ((z[5] < 4.0F) || (z[4] > 15.0F)) { + /* 'attitudeKalmanfilter:159' r(2)=10000; */ + r[1] = 10000.0F; + } + /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */ + /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */ + /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */ + /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */ + /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */ + /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:138' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:139' Z, Z, E, Z]; */ - /* 'attitudeKalmanfilter:141' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:143' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:144' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */ + /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:176' 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]; + iv5[i1 + 12 * i0]; } } } for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { - b_K_k[i + 6 * i0] = 0.0F; + c_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 + c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 * i0]; } } - } - for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { - f0 = 0.0F; + fv2[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - f0 += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0]; + fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0]; } + } + } - fv1[i + 6 * i0] = f0 + S_k[i + 6 * i0]; + b_K_k[0] = r[0]; + b_K_k[6] = 0.0F; + b_K_k[12] = 0.0F; + b_K_k[18] = 0.0F; + b_K_k[24] = 0.0F; + b_K_k[30] = 0.0F; + b_K_k[1] = 0.0F; + b_K_k[7] = r[0]; + b_K_k[13] = 0.0F; + b_K_k[19] = 0.0F; + b_K_k[25] = 0.0F; + b_K_k[31] = 0.0F; + b_K_k[2] = 0.0F; + b_K_k[8] = 0.0F; + b_K_k[14] = r[0]; + b_K_k[20] = 0.0F; + b_K_k[26] = 0.0F; + b_K_k[32] = 0.0F; + b_K_k[3] = 0.0F; + b_K_k[9] = 0.0F; + b_K_k[15] = 0.0F; + b_K_k[21] = r[1]; + b_K_k[27] = 0.0F; + b_K_k[33] = 0.0F; + b_K_k[4] = 0.0F; + b_K_k[10] = 0.0F; + b_K_k[16] = 0.0F; + b_K_k[22] = 0.0F; + b_K_k[28] = r[1]; + b_K_k[34] = 0.0F; + b_K_k[5] = 0.0F; + b_K_k[11] = 0.0F; + b_K_k[17] = 0.0F; + b_K_k[23] = 0.0F; + b_K_k[29] = 0.0F; + b_K_k[35] = r[1]; + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 6; i0++) { + c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; } } - c_mrdivide(d_P_apriori, fv1, b_K_k); + c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - /* 'attitudeKalmanfilter:147' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 6; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv5[i + 6 * i0] * x_apriori[i0]; + f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; } - b_r[i] = z[i] - f0; + b_z[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { - f0 += b_K_k[i + 12 * i0] * b_r[i0]; + f0 += c_K_k[i + 12 * i0] * b_z[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:148' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:180' 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++) { f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { - f0 += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0]; + f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -615,98 +936,137 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const 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]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + + 12 * i0]; } } } } else { - /* 'attitudeKalmanfilter:149' else */ - /* 'attitudeKalmanfilter:150' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ + /* 'attitudeKalmanfilter:181' else */ + /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:151' R=diag([r(1:3);r(7:9)]); */ + /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */ + /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */ + /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */ + /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */ + /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */ + /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:154' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:155' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:157' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:159' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:197' 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]; + } + } + } + for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { - b_K_k[i + 6 * i0] = 0.0F; + c_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 + + c_K_k[i + 6 * i0] += (real32_T)iv8[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++) { - f0 = 0.0F; + fv2[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - f0 += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0]; + fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * + i0]; } - - S_k[i + 6 * i0] = f0 + b_r[3 * (i + i0)]; } } - /* 'attitudeKalmanfilter:160' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { + b_K_k[0] = r[0]; + b_K_k[6] = 0.0F; + b_K_k[12] = 0.0F; + b_K_k[18] = 0.0F; + b_K_k[24] = 0.0F; + b_K_k[30] = 0.0F; + b_K_k[1] = 0.0F; + b_K_k[7] = r[0]; + b_K_k[13] = 0.0F; + b_K_k[19] = 0.0F; + b_K_k[25] = 0.0F; + b_K_k[31] = 0.0F; + b_K_k[2] = 0.0F; + b_K_k[8] = 0.0F; + b_K_k[14] = r[0]; + b_K_k[20] = 0.0F; + b_K_k[26] = 0.0F; + b_K_k[32] = 0.0F; + b_K_k[3] = 0.0F; + b_K_k[9] = 0.0F; + b_K_k[15] = 0.0F; + b_K_k[21] = r[2]; + b_K_k[27] = 0.0F; + b_K_k[33] = 0.0F; + b_K_k[4] = 0.0F; + b_K_k[10] = 0.0F; + b_K_k[16] = 0.0F; + b_K_k[22] = 0.0F; + b_K_k[28] = r[2]; + b_K_k[34] = 0.0F; + b_K_k[5] = 0.0F; + b_K_k[11] = 0.0F; + b_K_k[17] = 0.0F; + b_K_k[23] = 0.0F; + b_K_k[29] = 0.0F; + b_K_k[35] = r[2]; + for (i = 0; i < 6; 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_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; } } - c_mrdivide(d_P_apriori, S_k, b_K_k); + c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - /* 'attitudeKalmanfilter:163' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { - b_r[i] = z[i]; + b_z[i] = z[i]; } for (i = 0; i < 3; i++) { - b_r[i + 3] = z[i + 6]; + b_z[i + 3] = z[i + 6]; } for (i = 0; i < 6; i++) { - fv2[i] = 0.0F; + fv3[i] = 0.0F; for (i0 = 0; i0 < 12; i0++) { - fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; + fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0]; } - b_z[i] = b_r[i] - fv2[i]; + c_z[i] = b_z[i] - fv3[i]; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { - f0 += b_K_k[i + 12 * i0] * b_z[i0]; + f0 += c_K_k[i + 12 * i0] * c_z[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:164' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:201' 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++) { f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { - f0 += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; + f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -714,19 +1074,19 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const 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]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * + P_apriori[i1 + 12 * i0]; } } } } else { - /* 'attitudeKalmanfilter:165' else */ - /* 'attitudeKalmanfilter:166' x_aposteriori=x_apriori; */ + /* 'attitudeKalmanfilter:202' else */ + /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */ for (i = 0; i < 12; i++) { x_aposteriori[i] = x_apriori[i]; } - /* 'attitudeKalmanfilter:167' P_aposteriori=P_apriori; */ + /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */ memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); } } @@ -734,54 +1094,54 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } /* % euler anglels extraction */ - /* 'attitudeKalmanfilter:176' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ + /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ for (i = 0; i < 3; i++) { x_n_b[i] = -x_aposteriori[i + 6]; } rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); - /* 'attitudeKalmanfilter:177' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ + /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& - x_aposteriori[9]), m_n_b); + x_aposteriori[9]), wak); - /* 'attitudeKalmanfilter:179' y_n_b=cross(z_n_b,m_n_b); */ + /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */ for (i = 0; i < 3; i++) { - x_n_b[i] = m_n_b[i]; + x_n_b[i] = wak[i]; } - cross(z_n_b, x_n_b, m_n_b); + cross(z_n_b, x_n_b, wak); - /* 'attitudeKalmanfilter:180' y_n_b=y_n_b./norm(y_n_b); */ + /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */ for (i = 0; i < 3; i++) { - x_n_b[i] = m_n_b[i]; + x_n_b[i] = wak[i]; } - rdivide(x_n_b, norm(m_n_b), m_n_b); + rdivide(x_n_b, norm(wak), wak); - /* 'attitudeKalmanfilter:182' x_n_b=(cross(y_n_b,z_n_b)); */ - cross(m_n_b, z_n_b, x_n_b); + /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */ + cross(wak, z_n_b, x_n_b); - /* 'attitudeKalmanfilter:183' x_n_b=x_n_b./norm(x_n_b); */ + /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */ for (i = 0; i < 3; i++) { b_x_aposteriori_k[i] = x_n_b[i]; } rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); - /* 'attitudeKalmanfilter:189' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ for (i = 0; i < 3; i++) { Rot_matrix[i] = x_n_b[i]; - Rot_matrix[3 + i] = m_n_b[i]; + Rot_matrix[3 + i] = wak[i]; Rot_matrix[6 + i] = z_n_b[i]; } - /* 'attitudeKalmanfilter:193' phi=atan2f(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:194' theta=-asinf(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:195' psi=atan2f(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */ + /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */ + /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); - eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]); + eulerAngles[1] = -(real32_T)asin(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 old mode 100644 new mode 100755 index c93d7f4bb..afa63c1a9 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ @@ -29,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -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]); +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], 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 old mode 100644 new mode 100755 index 67b6fa422..7d620d7fa --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h old mode 100644 new mode 100755 index 610924d75..8b599be66 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c old mode 100644 new mode 100755 index c89602723..7f9727419 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h old mode 100644 new mode 100755 index a19630775..da84a5024 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h old mode 100644 new mode 100755 index eba83d0d9..30fd1e75e --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c old mode 100644 new mode 100755 index 42cd70971..84ada9002 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h old mode 100644 new mode 100755 index c90d6b3a5..e727f0684 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c old mode 100644 new mode 100755 index dbd44c6a8..b89ab58ef --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h old mode 100644 new mode 100755 index 325fd23ec..94fbe7671 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c old mode 100644 new mode 100755 index 0a540775a..a810f22e4 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h old mode 100644 new mode 100755 index d286eda5a..2d3b0d51f --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c old mode 100644 new mode 100755 index deb71dc60..0c418cc7b --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h old mode 100644 new mode 100755 index 3459cbdb5..60cf77b57 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/apps/attitude_estimator_ekf/codegen/rdivide.c old mode 100644 new mode 100755 index 11e4e6f1f..d035dae5e --- a/apps/attitude_estimator_ekf/codegen/rdivide.c +++ b/apps/attitude_estimator_ekf/codegen/rdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'rdivide' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/apps/attitude_estimator_ekf/codegen/rdivide.h old mode 100644 new mode 100755 index 4e2430374..4bbebebe2 --- a/apps/attitude_estimator_ekf/codegen/rdivide.h +++ b/apps/attitude_estimator_ekf/codegen/rdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'rdivide' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c old mode 100644 new mode 100755 index d20fa13fc..34164d104 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h old mode 100644 new mode 100755 index 83a355873..145373cd0 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c old mode 100644 new mode 100755 index d357102af..d84ca9573 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h old mode 100644 new mode 100755 index 415927709..65fdaa96f --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h old mode 100644 new mode 100755 index 608391608..356498363 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c old mode 100644 new mode 100755 index 69069562b..303d1d9d2 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h old mode 100644 new mode 100755 index 9686964ae..bd56b30d9 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h old mode 100644 new mode 100755 index 77fd3ec7a..9a5c96267 --- 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: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ @@ -26,8 +26,8 @@ * Number of bits: char: 8 short: 16 int: 32 * long: 32 native word size: 32 * Byte ordering: LittleEndian - * Signed integer division rounds to: Undefined - * Shift right on a signed integer as arithmetic shift: off + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on *=======================================================================*/ /*=======================================================================* -- cgit v1.2.3