aboutsummaryrefslogtreecommitdiff
path: root/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
diff options
context:
space:
mode:
Diffstat (limited to 'apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c')
-rwxr-xr-xapps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c463
1 files changed, 252 insertions, 211 deletions
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
index 86d872fd2..26a696af2 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
@@ -3,19 +3,19 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Sat Oct 13 17:51:09 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
+#include "rdivide.h"
#include "norm.h"
#include "cross.h"
#include "eye.h"
#include "mrdivide.h"
#include "diag.h"
-#include "power.h"
/* Type Definitions */
@@ -32,10 +32,24 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);
static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
{
real32_T y;
+ int32_T b_u0;
+ int32_T b_u1;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else if (rtIsInfF(u0) && rtIsInfF(u1)) {
- y = (real32_T)atan2f(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F);
+ if (u0 > 0.0F) {
+ b_u0 = 1;
+ } else {
+ b_u0 = -1;
+ }
+
+ if (u1 > 0.0F) {
+ b_u1 = 1;
+ } else {
+ b_u1 = -1;
+ }
+
+ 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;
@@ -45,7 +59,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;
@@ -60,30 +74,41 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
P_aposteriori[144])
{
- real32_T a[12];
- int32_T i;
- real32_T b_a[12];
- real32_T Q[144];
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 y_n_b[3];
int32_T i0;
- real32_T e_a[3];
real_T dv1[144];
real32_T A_lin[144];
real32_T b_A_lin[144];
int32_T i1;
- real32_T y;
real32_T P_apriori[144];
+ real32_T f0;
+ static const real32_T fv0[144] = { 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-10F, 0.0F, 0.0F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.1F, 0.0F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.1F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F, 0.0F,
+ 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.1F,
+ 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0001F, 0.0001F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+ 0.1F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+ 0.1F };
+
real32_T R[81];
real32_T b_P_apriori[108];
static const int8_T iv0[108] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
- 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 };
@@ -91,46 +116,45 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
real32_T K_k[108];
static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 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 fv1[81];
real32_T c_P_apriori[36];
static const int8_T iv2[36] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
- 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0 };
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
- real32_T fv1[36];
+ real32_T fv2[36];
static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0,
- 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+ 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 d_P_apriori[72];
static const int8_T iv4[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
- 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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,
- 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 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, 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, 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, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 1 };
static const int8_T iv7[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
- 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 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, 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 fv3[6];
real32_T b_z[6];
- real32_T b_y;
/* Extended Attitude Kalmanfilter */
/* */
@@ -146,32 +170,37 @@ 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, 2.0, a);
- for (i = 0; i < 12; i++) {
- b_a[i] = a[i] * dt;
- }
-
- diag(b_a, Q);
-
+ /* Q = diag(q.^2*dt); */
+ /* 'attitudeKalmanfilter:29' Q=[1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:30' 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:31' 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:32' 0, 0, 0, 1e-10, 0, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */
+ /* 'attitudeKalmanfilter:33' 0, 0, 0, 0, 1e-10, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */
+ /* 'attitudeKalmanfilter:34' 0, 0, 0, 0, 0, 1e-10, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:35' 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:36' 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:37' 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:38' 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0; */
+ /* 'attitudeKalmanfilter:39' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0; */
+ /* 'attitudeKalmanfilter:40' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1]; */
/* observation matrix */
- /* 'attitudeKalmanfilter:37' wx= x_aposteriori_k(1); */
- /* 'attitudeKalmanfilter:38' wy= x_aposteriori_k(2); */
- /* 'attitudeKalmanfilter:39' wz= x_aposteriori_k(3); */
- /* 'attitudeKalmanfilter:41' wox= x_aposteriori_k(4); */
- /* 'attitudeKalmanfilter:42' woy= x_aposteriori_k(5); */
- /* 'attitudeKalmanfilter:43' woz= x_aposteriori_k(6); */
- /* 'attitudeKalmanfilter:45' zex= x_aposteriori_k(7); */
- /* 'attitudeKalmanfilter:46' zey= x_aposteriori_k(8); */
- /* 'attitudeKalmanfilter:47' zez= x_aposteriori_k(9); */
- /* 'attitudeKalmanfilter:49' mux= x_aposteriori_k(10); */
- /* 'attitudeKalmanfilter:50' muy= x_aposteriori_k(11); */
- /* 'attitudeKalmanfilter:51' muz= x_aposteriori_k(12); */
- /* 'attitudeKalmanfilter:54' wk =[wx; */
- /* 'attitudeKalmanfilter:55' wy; */
- /* 'attitudeKalmanfilter:56' wz]; */
- /* 'attitudeKalmanfilter:58' wok =[wox;woy;woz]; */
- /* 'attitudeKalmanfilter:59' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
+ /* '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]'; */
O[0] = 0.0F;
O[1] = -x_aposteriori_k[2];
O[2] = x_aposteriori_k[1];
@@ -182,33 +211,36 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
O[7] = x_aposteriori_k[0];
O[8] = 0.0F;
- /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */
+ /* 'attitudeKalmanfilter:67' 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:61' muk =(eye(3)+O*dt)*[mux;muy;muz]; */
+ /* 'attitudeKalmanfilter:68' 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:63' EZ=[0,zez,-zey; */
- /* 'attitudeKalmanfilter:64' -zez,0,zex; */
- /* 'attitudeKalmanfilter:65' zey,-zex,0]'; */
- /* 'attitudeKalmanfilter:66' MA=[0,muz,-muy; */
- /* 'attitudeKalmanfilter:67' -muz,0,mux; */
- /* 'attitudeKalmanfilter:68' zey,-mux,0]'; */
- /* 'attitudeKalmanfilter:72' E=eye(3); */
- /* 'attitudeKalmanfilter:73' Z=zeros(3); */
- /* 'attitudeKalmanfilter:74' x_apriori=[wk;wok;zek;muk]; */
+ /* '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];
- z_n_b[0] = x_aposteriori_k[9];
- z_n_b[1] = x_aposteriori_k[10];
- z_n_b[2] = x_aposteriori_k[11];
+ 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];
@@ -216,28 +248,28 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
x_apriori[4] = x_aposteriori_k[4];
x_apriori[5] = x_aposteriori_k[5];
for (i = 0; i < 3; i++) {
- y_n_b[i] = 0.0F;
+ m_n_b[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
- y_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0];
+ m_n_b[i] += a[i + 3 * i0] * x_n_b[i0];
}
- e_a[i] = 0.0F;
+ z_n_b[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
- e_a[i] += d_a[i + 3 * i0] * z_n_b[i0];
+ z_n_b[i] += b_a[i + 3 * i0] * b_x_aposteriori_k[i0];
}
- x_apriori[i + 6] = y_n_b[i];
+ x_apriori[i + 6] = m_n_b[i];
}
for (i = 0; i < 3; i++) {
- x_apriori[i + 9] = e_a[i];
+ x_apriori[i + 9] = z_n_b[i];
}
- /* 'attitudeKalmanfilter:76' A_lin=[ Z, Z, Z, Z */
- /* 'attitudeKalmanfilter:77' Z, Z, Z, Z */
- /* 'attitudeKalmanfilter:78' EZ, Z, O, Z */
- /* 'attitudeKalmanfilter:79' MA, Z, Z, O]; */
- /* 'attitudeKalmanfilter:82' A_lin=eye(12)+A_lin*dt; */
+ /* '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; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 3; i0++) {
@@ -310,7 +342,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
}
}
- /* 'attitudeKalmanfilter:88' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
+ /* 'attitudeKalmanfilter:98' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
A_lin[i + 12 * i0] = 0.0F;
@@ -323,28 +355,28 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
- y += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
+ f0 += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
}
- P_apriori[i + 12 * i0] = y + Q[i + 12 * i0];
+ P_apriori[i + 12 * i0] = f0 + fv0[i + 12 * i0];
}
}
/* %update */
- /* 'attitudeKalmanfilter:92' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
+ /* 'attitudeKalmanfilter:102' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {
- /* 'attitudeKalmanfilter:93' R=diag(r); */
- b_diag(r, R);
+ /* 'attitudeKalmanfilter:103' R=diag(r); */
+ diag(r, R);
/* observation matrix */
- /* 'attitudeKalmanfilter:96' H_k=[ E, E, Z, Z; */
- /* 'attitudeKalmanfilter:97' Z, Z, E, Z; */
- /* 'attitudeKalmanfilter:98' Z, Z, Z, E]; */
- /* 'attitudeKalmanfilter:100' y_k=z(1:9)-H_k*x_apriori; */
- /* 'attitudeKalmanfilter:102' S_k=H_k*P_apriori*H_k'+R; */
- /* 'attitudeKalmanfilter:103' K_k=(P_apriori*H_k'/(S_k)); */
+ /* 'attitudeKalmanfilter:106' H_k=[ E, Es, 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)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 9; i0++) {
b_P_apriori[i + 12 * i0] = 0.0F;
@@ -366,46 +398,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 9; i++) {
for (i0 = 0; i0 < 9; i0++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
- y += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0];
+ f0 += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0];
}
- fv0[i + 9 * i0] = y + R[i + 9 * i0];
+ fv1[i + 9 * i0] = f0 + R[i + 9 * i0];
}
}
- mrdivide(b_P_apriori, fv0, K_k);
+ mrdivide(b_P_apriori, fv1, K_k);
- /* 'attitudeKalmanfilter:106' x_aposteriori=x_apriori+K_k*y_k; */
+ /* 'attitudeKalmanfilter:116' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 9; i++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
- y += (real32_T)iv1[i + 9 * i0] * x_apriori[i0];
+ f0 += (real32_T)iv1[i + 9 * i0] * x_apriori[i0];
}
- c_a[i] = z[i] - y;
+ a[i] = z[i] - f0;
}
for (i = 0; i < 12; i++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i0 = 0; i0 < 9; i0++) {
- y += K_k[i + 12 * i0] * c_a[i0];
+ f0 += K_k[i + 12 * i0] * a[i0];
}
- x_aposteriori[i] = x_apriori[i] + y;
+ x_aposteriori[i] = x_apriori[i] + f0;
}
- /* 'attitudeKalmanfilter:107' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
+ /* 'attitudeKalmanfilter:117' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i1 = 0; i1 < 9; i1++) {
- y += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0];
+ f0 += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0];
}
- Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -413,22 +445,23 @@ 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:108' else */
- /* 'attitudeKalmanfilter:109' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
+ /* 'attitudeKalmanfilter:118' else */
+ /* 'attitudeKalmanfilter:119' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {
- /* 'attitudeKalmanfilter:110' R=diag(r(1:3)); */
- c_diag(*(real32_T (*)[3])&r[0], O);
+ /* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */
+ b_diag(*(real32_T (*)[3])&r[0], O);
/* observation matrix */
- /* 'attitudeKalmanfilter:113' H_k=[ E, E, Z, Z]; */
- /* 'attitudeKalmanfilter:115' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
- /* 'attitudeKalmanfilter:117' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
- /* 'attitudeKalmanfilter:118' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */
+ /* 'attitudeKalmanfilter:123' H_k=[ E, Es, 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)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 3; i0++) {
c_P_apriori[i + 12 * i0] = 0.0F;
@@ -441,9 +474,9 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
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)iv3[i + 3 * i1] * P_apriori[i1 + 12 *
i0];
}
}
@@ -451,46 +484,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
- y += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];
+ f0 += fv2[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0];
}
- c_a[i + 3 * i0] = y + O[i + 3 * i0];
+ a[i + 3 * i0] = f0 + O[i + 3 * i0];
}
}
- b_mrdivide(c_P_apriori, c_a, S_k);
+ b_mrdivide(c_P_apriori, a, S_k);
- /* 'attitudeKalmanfilter:121' x_aposteriori=x_apriori+K_k*y_k; */
+ /* 'attitudeKalmanfilter:131' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 3; i++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
- y += (real32_T)iv3[i + 3 * i0] * x_apriori[i0];
+ f0 += (real32_T)iv3[i + 3 * i0] * x_apriori[i0];
}
- x_n_b[i] = z[i] - y;
+ x_n_b[i] = z[i] - f0;
}
for (i = 0; i < 12; i++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
- y += S_k[i + 12 * i0] * x_n_b[i0];
+ f0 += S_k[i + 12 * i0] * x_n_b[i0];
}
- x_aposteriori[i] = x_apriori[i] + y;
+ x_aposteriori[i] = x_apriori[i] + f0;
}
- /* 'attitudeKalmanfilter:122' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */
+ /* 'attitudeKalmanfilter:132' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
- y += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0];
+ f0 += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0];
}
- Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -498,25 +531,25 @@ 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:123' else */
- /* 'attitudeKalmanfilter:124' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */
+ /* 'attitudeKalmanfilter:133' else */
+ /* 'attitudeKalmanfilter:134' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */
if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))
{
- /* 'attitudeKalmanfilter:125' R=diag(r(1:6)); */
- d_diag(*(real32_T (*)[6])&r[0], S_k);
+ /* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */
+ c_diag(*(real32_T (*)[6])&r[0], S_k);
/* observation matrix */
- /* 'attitudeKalmanfilter:128' H_k=[ E, E, Z, Z; */
- /* 'attitudeKalmanfilter:129' Z, Z, E, Z]; */
- /* 'attitudeKalmanfilter:131' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
- /* 'attitudeKalmanfilter:133' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
- /* 'attitudeKalmanfilter:134' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
+ /* 'attitudeKalmanfilter:138' H_k=[ E, Es, 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)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 6; i0++) {
d_P_apriori[i + 12 * i0] = 0.0F;
@@ -539,46 +572,46 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 6; i++) {
for (i0 = 0; i0 < 6; i0++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
- y += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0];
+ f0 += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0];
}
- fv1[i + 6 * i0] = y + S_k[i + 6 * i0];
+ fv2[i + 6 * i0] = f0 + S_k[i + 6 * i0];
}
}
- c_mrdivide(d_P_apriori, fv1, b_K_k);
+ c_mrdivide(d_P_apriori, fv2, b_K_k);
- /* 'attitudeKalmanfilter:137' x_aposteriori=x_apriori+K_k*y_k; */
+ /* 'attitudeKalmanfilter:147' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 6; i++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
- y += (real32_T)iv5[i + 6 * i0] * x_apriori[i0];
+ f0 += (real32_T)iv5[i + 6 * i0] * x_apriori[i0];
}
- b_r[i] = z[i] - y;
+ b_r[i] = z[i] - f0;
}
for (i = 0; i < 12; i++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i0 = 0; i0 < 6; i0++) {
- y += b_K_k[i + 12 * i0] * b_r[i0];
+ f0 += b_K_k[i + 12 * i0] * b_r[i0];
}
- x_aposteriori[i] = x_apriori[i] + y;
+ x_aposteriori[i] = x_apriori[i] + f0;
}
- /* 'attitudeKalmanfilter:138' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
+ /* 'attitudeKalmanfilter:148' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i1 = 0; i1 < 6; i1++) {
- y += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0];
+ f0 += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0];
}
- Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -586,22 +619,22 @@ 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:139' else */
- /* 'attitudeKalmanfilter:140' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */
+ /* 'attitudeKalmanfilter:149' else */
+ /* 'attitudeKalmanfilter:150' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */
if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))
{
- /* 'attitudeKalmanfilter:141' R=diag([r(1:3);r(7:9)]); */
+ /* 'attitudeKalmanfilter:151' R=diag([r(1:3);r(7:9)]); */
/* observation matrix */
- /* 'attitudeKalmanfilter:144' H_k=[ E, E, Z, Z; */
- /* 'attitudeKalmanfilter:145' Z, Z, Z, E]; */
- /* 'attitudeKalmanfilter:147' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
- /* 'attitudeKalmanfilter:149' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
+ /* 'attitudeKalmanfilter:154' H_k=[ E, Es, 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); */
for (i = 0; i < 6; i++) {
for (i0 = 0; i0 < 12; i0++) {
b_K_k[i + 6 * i0] = 0.0F;
@@ -619,16 +652,16 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
for (i = 0; i < 6; i++) {
for (i0 = 0; i0 < 6; i0++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
- y += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0];
+ f0 += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0];
}
- S_k[i + 6 * i0] = y + b_r[3 * (i + i0)];
+ S_k[i + 6 * i0] = f0 + b_r[3 * (i + i0)];
}
}
- /* 'attitudeKalmanfilter:150' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
+ /* 'attitudeKalmanfilter:160' 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;
@@ -641,7 +674,7 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
c_mrdivide(d_P_apriori, S_k, b_K_k);
- /* 'attitudeKalmanfilter:153' x_aposteriori=x_apriori+K_k*y_k; */
+ /* 'attitudeKalmanfilter:163' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 3; i++) {
b_r[i] = z[i];
}
@@ -651,33 +684,33 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
}
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)iv6[i + 6 * i0] * x_apriori[i0];
}
- b_z[i] = b_r[i] - fv2[i];
+ b_z[i] = b_r[i] - fv3[i];
}
for (i = 0; i < 12; i++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i0 = 0; i0 < 6; i0++) {
- y += b_K_k[i + 12 * i0] * b_z[i0];
+ f0 += b_K_k[i + 12 * i0] * b_z[i0];
}
- x_aposteriori[i] = x_apriori[i] + y;
+ x_aposteriori[i] = x_apriori[i] + f0;
}
- /* 'attitudeKalmanfilter:154' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
+ /* 'attitudeKalmanfilter:164' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
- y = 0.0F;
+ f0 = 0.0F;
for (i1 = 0; i1 < 6; i1++) {
- y += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
+ f0 += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
}
- Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y;
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
@@ -685,66 +718,74 @@ 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:155' else */
- /* 'attitudeKalmanfilter:156' x_aposteriori=x_apriori; */
+ /* 'attitudeKalmanfilter:165' else */
+ /* 'attitudeKalmanfilter:166' x_aposteriori=x_apriori; */
for (i = 0; i < 12; i++) {
x_aposteriori[i] = x_apriori[i];
}
- /* 'attitudeKalmanfilter:157' P_aposteriori=P_apriori; */
- memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 144U * sizeof
- (real32_T));
+ /* 'attitudeKalmanfilter:167' P_aposteriori=P_apriori; */
+ memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));
}
}
}
}
/* % euler anglels extraction */
- /* 'attitudeKalmanfilter:166' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
- y = norm(*(real32_T (*)[3])&x_aposteriori[6]);
+ /* 'attitudeKalmanfilter:176' 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];
+ }
- /* 'attitudeKalmanfilter:167' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */
- b_y = norm(*(real32_T (*)[3])&x_aposteriori[9]);
+ rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);
- /* 'attitudeKalmanfilter:169' y_n_b=cross(z_n_b,m_n_b); */
+ /* 'attitudeKalmanfilter:177' 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);
+
+ /* 'attitudeKalmanfilter:179' y_n_b=cross(z_n_b,m_n_b); */
+ for (i = 0; i < 3; i++) {
+ x_n_b[i] = m_n_b[i];
+ }
+
+ cross(z_n_b, x_n_b, m_n_b);
+
+ /* 'attitudeKalmanfilter:180' y_n_b=y_n_b./norm(y_n_b); */
for (i = 0; i < 3; i++) {
- z_n_b[i] = -x_aposteriori[i + 6] / y;
- x_n_b[i] = x_aposteriori[i + 9] / b_y;
+ x_n_b[i] = m_n_b[i];
}
- cross(z_n_b, x_n_b, y_n_b);
+ rdivide(x_n_b, norm(m_n_b), m_n_b);
+
+ /* 'attitudeKalmanfilter:182' x_n_b=(cross(y_n_b,z_n_b)); */
+ cross(m_n_b, z_n_b, x_n_b);
- /* 'attitudeKalmanfilter:170' y_n_b=y_n_b./norm(y_n_b); */
- y = norm(y_n_b);
+ /* 'attitudeKalmanfilter:183' x_n_b=x_n_b./norm(x_n_b); */
for (i = 0; i < 3; i++) {
- y_n_b[i] /= y;
+ b_x_aposteriori_k[i] = x_n_b[i];
}
- /* 'attitudeKalmanfilter:172' x_n_b=(cross(y_n_b,z_n_b)); */
- cross(y_n_b, z_n_b, x_n_b);
+ rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);
- /* 'attitudeKalmanfilter:173' x_n_b=x_n_b./norm(x_n_b); */
- y = norm(x_n_b);
+ /* 'attitudeKalmanfilter:189' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
for (i = 0; i < 3; i++) {
- /* 'attitudeKalmanfilter:179' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
- Rot_matrix[i] = x_n_b[i] / y;
- Rot_matrix[3 + i] = y_n_b[i];
+ Rot_matrix[i] = x_n_b[i];
+ Rot_matrix[3 + i] = m_n_b[i];
Rot_matrix[6 + i] = z_n_b[i];
}
- /* 'attitudeKalmanfilter:183' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
- /* 'attitudeKalmanfilter:184' theta=-asin(Rot_matrix(1,3)); */
- /* 'attitudeKalmanfilter:185' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
- /* 'attitudeKalmanfilter:186' eulerAngles=[phi;theta;psi]; */
+ /* 'attitudeKalmanfilter:193' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
+ /* 'attitudeKalmanfilter:194' theta=-asin(Rot_matrix(1,3)); */
+ /* 'attitudeKalmanfilter:195' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
+ /* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */
eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);
eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
}
-/* End of code generation (attitudeKalmanfilter.c) */
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-02 18:17:22 +0000