diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-21 17:30:53 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-21 17:30:53 +0100 |
| commit | ac215185a9604a88665d0b5b8382659f8cedaf56 (patch) | |
| tree | f7ba1eca830745fd7c002f954de27d38ea0995da /apps/attitude_estimator_ekf | |
| parent | 48e497e4069a2f8773d90f2d1887967a81e487d8 (diff) | |
| download | px4-firmware-ac215185a9604a88665d0b5b8382659f8cedaf56.tar.gz px4-firmware-ac215185a9604a88665d0b5b8382659f8cedaf56.tar.bz2 px4-firmware-ac215185a9604a88665d0b5b8382659f8cedaf56.zip | |
Better attitude filter, not sensitive to sudden accelerations
Diffstat (limited to 'apps/attitude_estimator_ekf')
29 files changed, 652 insertions, 328 deletions
diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index 4d531867c..734af7cc1 100644..100755 --- 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 index 1c3b9976b..7ca77e513 100644..100755 --- 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 index 91fec9ccb..5b7756611 100644..100755 --- 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 index b315d5fe8..09817d58e 100644..100755 --- 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 index 04f6ea267..9e97ad11a 100644..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: 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 index c93d7f4bb..afa63c1a9 100644..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: 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 index 67b6fa422..7d620d7fa 100644..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: 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 index 610924d75..8b599be66 100644..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: 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 index c89602723..7f9727419 100644..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: 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 index a19630775..da84a5024 100644..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: 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 index eba83d0d9..30fd1e75e 100644..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: 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 index 42cd70971..84ada9002 100644..100755 --- a/apps/attitude_estimator_ekf/codegen/cross.c +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -3,7 +3,7 @@ *
* Code generation for function 'cross'
*
- * C source code generated on: 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 index c90d6b3a5..e727f0684 100644..100755 --- a/apps/attitude_estimator_ekf/codegen/cross.h +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -3,7 +3,7 @@ *
* Code generation for function 'cross'
*
- * C source code generated on: 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 index dbd44c6a8..b89ab58ef 100644..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: 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 index 325fd23ec..94fbe7671 100644..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: 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 index 0a540775a..a810f22e4 100644..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: 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 index d286eda5a..2d3b0d51f 100644..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: 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 index deb71dc60..0c418cc7b 100644..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: 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 index 3459cbdb5..60cf77b57 100644..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: 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 index 11e4e6f1f..d035dae5e 100644..100755 --- 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 index 4e2430374..4bbebebe2 100644..100755 --- 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 index d20fa13fc..34164d104 100644..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: 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 index 83a355873..145373cd0 100644..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: 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 index d357102af..d84ca9573 100644..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: 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 index 415927709..65fdaa96f 100644..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: 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 index 608391608..356498363 100644..100755 --- a/apps/attitude_estimator_ekf/codegen/rt_defines.h +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: 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 index 69069562b..303d1d9d2 100644..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: 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 index 9686964ae..bd56b30d9 100644..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: 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 index 77fd3ec7a..9a5c96267 100644..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: 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
*=======================================================================*/
/*=======================================================================*
|