diff options
Diffstat (limited to 'apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c')
| -rwxr-xr-x | apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c | 635 |
1 files changed, 635 insertions, 0 deletions
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c new file mode 100755 index 000000000..4e275e3ee --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -0,0 +1,635 @@ +/*
+ * attitudeKalmanfilter.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "norm.h"
+#include "eye.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+ */
+void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T
+ x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T
+ knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
+ P_aposteriori[144])
+{
+ real32_T R_temp[9];
+ real_T dv0[9];
+ real_T dv1[9];
+ int32_T i;
+ int32_T i0;
+ real32_T A_pred[144];
+ real32_T x_apriori[12];
+ real32_T b_A_pred[144];
+ int32_T i1;
+ real32_T c_A_pred[144];
+ static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
+
+ real32_T P_apriori[144];
+ real32_T b_P_apriori[108];
+ static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1 };
+
+ real32_T K_k[108];
+ static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+ real32_T fv0[81];
+ real32_T fv1[81];
+ real32_T fv2[81];
+ real32_T B;
+ real_T dv2[144];
+ real32_T b_B;
+ real32_T earth_z[3];
+ real32_T y[3];
+ real32_T earth_x[3];
+
+ /* Extended Attitude Kalmanfilter */
+ /* */
+ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
+ /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
+ /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+ /* */
+ /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
+ /* */
+ /* Example.... */
+ /* */
+ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */
+ /* %define the matrices */
+ /* 'attitudeKalmanfilter:19' acc_ProcessNoise=knownConst(1); */
+ /* 'attitudeKalmanfilter:20' mag_ProcessNoise=knownConst(2); */
+ /* 'attitudeKalmanfilter:21' ratesOffset_ProcessNoise=knownConst(3); */
+ /* 'attitudeKalmanfilter:22' rates_ProcessNoise=knownConst(4); */
+ /* 'attitudeKalmanfilter:25' acc_MeasurementNoise=knownConst(5); */
+ /* 'attitudeKalmanfilter:26' mag_MeasurementNoise=knownConst(6); */
+ /* 'attitudeKalmanfilter:27' rates_MeasurementNoise=knownConst(7); */
+ /* process noise covariance matrix */
+ /* 'attitudeKalmanfilter:30' Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:31' zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:32' zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3); */
+ /* 'attitudeKalmanfilter:33' zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise]; */
+ /* measurement noise covariance matrix */
+ /* 'attitudeKalmanfilter:36' R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:37' zeros(3), eye(3)*mag_MeasurementNoise, zeros(3); */
+ /* 'attitudeKalmanfilter:38' zeros(3), zeros(3), eye(3)*rates_MeasurementNoise]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:42' H_k=[ eye(3), zeros(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:43' zeros(3), eye(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:44' zeros(3), zeros(3), eye(3), eye(3)]; */
+ /* compute A(t,w) */
+ /* x_aposteriori_k[10,11,12] should be [p,q,r] */
+ /* R_temp=[1,-r, q */
+ /* r, 1, -p */
+ /* -q, p, 1] */
+ /* 'attitudeKalmanfilter:53' R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); */
+ /* 'attitudeKalmanfilter:54' dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); */
+ /* 'attitudeKalmanfilter:55' -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1]; */
+ R_temp[0] = 1.0F;
+ R_temp[3] = -dt * x_aposteriori_k[11];
+ R_temp[6] = dt * x_aposteriori_k[10];
+ R_temp[1] = dt * x_aposteriori_k[11];
+ R_temp[4] = 1.0F;
+ R_temp[7] = -dt * x_aposteriori_k[9];
+ R_temp[2] = -dt * x_aposteriori_k[10];
+ R_temp[5] = dt * x_aposteriori_k[9];
+ R_temp[8] = 1.0F;
+
+ /* strange, should not be transposed */
+ /* 'attitudeKalmanfilter:58' A_pred=[R_temp', zeros(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:59' zeros(3), R_temp', zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:60' zeros(3), zeros(3), eye(3), zeros(3); */
+ /* 'attitudeKalmanfilter:61' zeros(3), zeros(3), zeros(3), eye(3)]; */
+ eye(dv0);
+ eye(dv1);
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * i] = R_temp[i + 3 * i0];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * (i + 3)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * (i + 6)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * (i + 9)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i];
+ }
+ }
+
+ /* %prediction step */
+ /* 'attitudeKalmanfilter:64' x_apriori=A_pred*x_aposteriori_k; */
+ for (i = 0; i < 12; i++) {
+ x_apriori[i] = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ x_apriori[i] += A_pred[i + 12 * i0] * x_aposteriori_k[i0];
+ }
+ }
+
+ /* linearization */
+ /* 'attitudeKalmanfilter:67' acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2); */
+ /* 'attitudeKalmanfilter:68' -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1); */
+ /* 'attitudeKalmanfilter:69' dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0]; */
+ /* 'attitudeKalmanfilter:71' mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5); */
+ /* 'attitudeKalmanfilter:72' -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4); */
+ /* 'attitudeKalmanfilter:73' dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0]; */
+ /* 'attitudeKalmanfilter:75' A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat'; */
+ /* 'attitudeKalmanfilter:76' zeros(3), R_temp', zeros(3), mag_temp_mat'; */
+ /* 'attitudeKalmanfilter:77' zeros(3), zeros(3), eye(3), zeros(3); */
+ /* 'attitudeKalmanfilter:78' zeros(3), zeros(3), zeros(3), eye(3)]; */
+ eye(dv0);
+ eye(dv1);
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * i] = R_temp[i + 3 * i0];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * (i + 3)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * (i + 6)] = 0.0F;
+ }
+ }
+
+ A_pred[108] = 0.0F;
+ A_pred[109] = dt * x_aposteriori_k[2];
+ A_pred[110] = -dt * x_aposteriori_k[1];
+ A_pred[120] = -dt * x_aposteriori_k[2];
+ A_pred[121] = 0.0F;
+ A_pred[122] = dt * x_aposteriori_k[0];
+ A_pred[132] = dt * x_aposteriori_k[1];
+ A_pred[133] = -dt * x_aposteriori_k[0];
+ A_pred[134] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+ }
+ }
+
+ A_pred[111] = 0.0F;
+ A_pred[112] = dt * x_aposteriori_k[5];
+ A_pred[113] = -dt * x_aposteriori_k[4];
+ A_pred[123] = -dt * x_aposteriori_k[5];
+ A_pred[124] = 0.0F;
+ A_pred[125] = dt * x_aposteriori_k[3];
+ A_pred[135] = dt * x_aposteriori_k[4];
+ A_pred[136] = -dt * x_aposteriori_k[3];
+ A_pred[137] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i];
+ }
+ }
+
+ /* 'attitudeKalmanfilter:81' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ b_A_pred[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ b_A_pred[i + 12 * i0] += A_pred[i + 12 * i1] * P_aposteriori_k[i1 + 12 *
+ i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 12; i0++) {
+ c_A_pred[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_A_pred[i + 12 * i0] += b_A_pred[i + 12 * i1] * A_pred[i0 + 12 * i1];
+ }
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[0];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[i0 + 12 * (i + 3)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[i0 + 12 * (i + 6)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[i0 + 12 * (i + 9)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * i) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] *
+ knownConst[1];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * i) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] *
+ knownConst[2];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * i) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)iv0[i0 + 3 * i] *
+ knownConst[3];
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_apriori[i0 + 12 * i] = c_A_pred[i0 + 12 * i] + b_A_pred[i0 + 12 * i];
+ }
+ }
+
+ /* %update step */
+ /* 'attitudeKalmanfilter:86' y_k=z_k-H_k*x_apriori; */
+ /* 'attitudeKalmanfilter:87' S_k=H_k*P_apriori*H_k'+R; */
+ /* 'attitudeKalmanfilter:88' K_k=(P_apriori*H_k'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ b_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 +
+ 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ K_k[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 9; i0++) {
+ fv0[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[i0 + 9 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[4];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[i0 + 9 * (i + 3)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[i0 + 9 * (i + 6)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * i) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * knownConst[5];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * (i + 6)) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * i) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * knownConst[6];
+ }
+ }
+
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ fv2[i0 + 9 * i] = fv0[i0 + 9 * i] + fv1[i0 + 9 * i];
+ }
+ }
+
+ mrdivide(b_P_apriori, fv2, K_k);
+
+ /* 'attitudeKalmanfilter:91' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 9; i++) {
+ B = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ B += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];
+ }
+
+ R_temp[i] = z_k[i] - B;
+ }
+
+ for (i = 0; i < 12; i++) {
+ B = 0.0F;
+ for (i0 = 0; i0 < 9; i0++) {
+ B += K_k[i + 12 * i0] * R_temp[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + B;
+ }
+
+ /* 'attitudeKalmanfilter:92' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
+ b_eye(dv2);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ B = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ B += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];
+ }
+
+ A_pred[i + 12 * i0] = (real32_T)dv2[i + 12 * i0] - B;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += A_pred[i + 12 * i1] * P_apriori[i1 + 12 *
+ i0];
+ }
+ }
+ }
+
+ /* %Rotation matrix generation */
+ /* 'attitudeKalmanfilter:97' earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); */
+ B = norm(*(real32_T (*)[3])&x_aposteriori[0]);
+
+ /* 'attitudeKalmanfilter:98' earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); */
+ b_B = norm(*(real32_T (*)[3])&x_aposteriori[3]);
+ for (i = 0; i < 3; i++) {
+ earth_z[i] = x_aposteriori[i] / B;
+ y[i] = x_aposteriori[i + 3] / b_B;
+ }
+
+ earth_x[0] = earth_z[1] * y[2] - earth_z[2] * y[1];
+ earth_x[1] = earth_z[2] * y[0] - earth_z[0] * y[2];
+ earth_x[2] = earth_z[0] * y[1] - earth_z[1] * y[0];
+
+ /* 'attitudeKalmanfilter:99' earth_y=cross(earth_x,earth_z); */
+ /* 'attitudeKalmanfilter:101' Rot_matrix=[earth_x,earth_y,earth_z]; */
+ y[0] = earth_x[1] * earth_z[2] - earth_x[2] * earth_z[1];
+ y[1] = earth_x[2] * earth_z[0] - earth_x[0] * earth_z[2];
+ y[2] = earth_x[0] * earth_z[1] - earth_x[1] * earth_z[0];
+ for (i = 0; i < 3; i++) {
+ Rot_matrix[i] = earth_x[i];
+ Rot_matrix[3 + i] = y[i];
+ Rot_matrix[6 + i] = earth_z[i];
+ }
+}
+
+/* End of code generation (attitudeKalmanfilter.c) */
|