diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-19 22:36:41 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-19 22:36:41 +0200 |
| commit | efcf146b6d22600341b55283b39f8b0a846dee09 (patch) | |
| tree | b2fa5d6de973ef54361227ca92628b62cf7df5f4 /apps/attitude_estimator_ekf | |
| parent | 291f4f3a33e6428b23624b1ffe12fec1015816cd (diff) | |
| download | px4-firmware-efcf146b6d22600341b55283b39f8b0a846dee09.tar.gz px4-firmware-efcf146b6d22600341b55283b39f8b0a846dee09.tar.bz2 px4-firmware-efcf146b6d22600341b55283b39f8b0a846dee09.zip | |
Updated EKF filter, untested
Diffstat (limited to 'apps/attitude_estimator_ekf')
34 files changed, 1499 insertions, 846 deletions
diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index cad20d375..b4d62ed14 100644 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -44,7 +44,10 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rt_nonfinite.c \ codegen/rtGetInf.c \ codegen/rtGetNaN.c \ - codegen/norm.c + codegen/norm.c \ + codegen/find.c \ + codegen/diag.c \ + codegen/cross.c # XXX this is *horribly* broken INCLUDES += $(TOPDIR)/../mavlink/include/mavlink diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 09cbdd4e9..6c18b044e 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -80,21 +80,35 @@ static float dt = 1; /* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ static float z_k[9] = {0}; /**< Measurement vector */ -static float x_aposteriori[12] = {0}; /**< */ -static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f +static float x_aposteriori_k[9] = {0}; /**< */ +static float x_aposteriori[9] = {0}; +static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + }; +static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, }; /**< init: diagonal matrix with big values */ -static float knownConst[7] = {1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ +static float knownConst[20] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ static float Rot_matrix[9] = {1.f, 0, 0, 0, 1.f, 0, 0, 0, 1.f @@ -203,13 +217,31 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) /* * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ + + //extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], + // const int32_T z_k_sizes[1], const real32_T u[4], + // const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], + // const real32_T knownConst[20], real32_T eulerAngles[3], + // real32_T Rot_matrix[9], real32_T x_aposteriori[9], + // real32_T P_aposteriori[81]); + + int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; + float euler[3]; + int32_t z_k_sizes = 9; + float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + uint64_t timing_start = hrt_absolute_time(); - attitudeKalmanfilter(dt, z_k, x_aposteriori, P_aposteriori, knownConst, Rot_matrix, x_aposteriori, P_aposteriori); + attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, + Rot_matrix, x_aposteriori, P_aposteriori); + /* swap values for next iteration */ + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); uint64_t timing_diff = hrt_absolute_time() - timing_start; /* print rotation matrix every 200th time */ if (printcounter % 200 == 0) { printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", euler[0], euler[1], euler[2]); printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 4e275e3ee..321e6874d 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:22 2012
*
*/
@@ -11,8 +11,11 @@ #include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "norm.h"
+#include "cross.h"
#include "eye.h"
#include "mrdivide.h"
+#include "diag.h"
+#include "find.h"
/* Type Definitions */
@@ -23,613 +26,691 @@ /* Variable Definitions */
/* Function Declarations */
+static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);
/* Function Definitions */
+static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
+{
+ real32_T y;
+ if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
+ y = ((real32_T)rtNaN);
+ } else if (rtIsInfF(u0) && rtIsInfF(u1)) {
+ y = (real32_T)atan2(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F);
+ } else if (u1 == 0.0F) {
+ if (u0 > 0.0F) {
+ y = RT_PIF / 2.0F;
+ } else if (u0 < 0.0F) {
+ y = -(RT_PIF / 2.0F);
+ } else {
+ y = 0.0F;
+ }
+ } else {
+ y = (real32_T)atan2(u0, u1);
+ }
+
+ return y;
+}
/*
- * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+ * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,updVect,z_k,u,x_aposteriori_k,P_aposteriori_k,knownConst)
*/
-void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T
- x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T
- knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
- P_aposteriori[144])
+void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T
+ z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T
+ x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T
+ knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T
+ x_aposteriori[9], real32_T P_aposteriori[81])
{
+ int32_T udpIndVect_sizes;
+ real_T udpIndVect_data[9];
real32_T R_temp[9];
real_T dv0[9];
- real_T dv1[9];
- int32_T i;
+ int32_T ib;
int32_T i0;
- real32_T A_pred[144];
- real32_T x_apriori[12];
- real32_T b_A_pred[144];
+ real32_T fv0[81];
+ real32_T b_knownConst[27];
+ real32_T fv1[9];
+ real32_T c_knownConst[9];
+ real_T dv1[9];
+ real_T dv2[9];
+ real32_T A_lin[81];
+ real32_T x_n_b[3];
+ real32_T K_k_data[81];
int32_T i1;
- real32_T c_A_pred[144];
+ real32_T b_A_lin[81];
static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
- real32_T P_apriori[144];
- real32_T b_P_apriori[108];
- static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
- 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1 };
-
- real32_T K_k[108];
- static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+ real32_T P_apriori[81];
+ int32_T ia;
+ static const int8_T H_k_full[81] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
- real32_T fv0[81];
- real32_T fv1[81];
- real32_T fv2[81];
- real32_T B;
- real_T dv2[144];
- real32_T b_B;
- real32_T earth_z[3];
- real32_T y[3];
- real32_T earth_x[3];
+ int8_T H_k_data[81];
+ int32_T accUpt;
+ int32_T magUpt;
+ real32_T y;
+ real32_T y_k_data[9];
+ int32_T P_apriori_sizes[2];
+ int32_T H_k_sizes[2];
+ int32_T K_k_sizes[2];
+ real32_T b_y[9];
+ real_T dv3[81];
+ real32_T c_y;
+ real32_T z_n_b[3];
+ real32_T y_n_b[3];
/* Extended Attitude Kalmanfilter */
- /* */
+ /* */
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
/* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
/* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
/* */
/* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
/* */
- /* Example.... */
+ /* Example.... */
/* */
/* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */
/* %define the matrices */
- /* 'attitudeKalmanfilter:19' acc_ProcessNoise=knownConst(1); */
- /* 'attitudeKalmanfilter:20' mag_ProcessNoise=knownConst(2); */
- /* 'attitudeKalmanfilter:21' ratesOffset_ProcessNoise=knownConst(3); */
- /* 'attitudeKalmanfilter:22' rates_ProcessNoise=knownConst(4); */
- /* 'attitudeKalmanfilter:25' acc_MeasurementNoise=knownConst(5); */
- /* 'attitudeKalmanfilter:26' mag_MeasurementNoise=knownConst(6); */
- /* 'attitudeKalmanfilter:27' rates_MeasurementNoise=knownConst(7); */
+ /* tpred=0.005; */
+ /* */
+ /* A=[ 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */
+ /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */
+ /* -1, 1, 0, 0, 0, 0, 0, 0, 0; */
+ /* 0, -1, 1, 0, 0, 0, 0, 0, 0; */
+ /* 1, 0, -1, 0, 0, 0, 0, 0, 0; */
+ /* -1, 1, 0, 0, 0, 0, 0, 0, 0]; */
+ /* */
+ /* */
+ /* b=[70, 0, 0; */
+ /* 0, 70, 0; */
+ /* 0, 0, 0; */
+ /* 0, 0, 0; */
+ /* 0, 0, 0; */
+ /* 0, 0, 0; */
+ /* 0, 0, 0; */
+ /* 0, 0, 0; */
+ /* 0, 0, 0]; */
+ /* */
+ /* */
+ /* C=[1, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 0, 1, 0, 0, 0, 0, 0, 0, 0; */
+ /* 0, 0, 1, 0, 0, 0, 0, 0, 0; */
+ /* 0, 0, 0, 1, 0, 0, 0, 0, 0; */
+ /* 0, 0, 0, 0, 1, 0, 0, 0, 0; */
+ /* 0, 0, 0, 0, 0, 1, 0, 0, 0; */
+ /* 0, 0, 0, 0, 0, 0, 1, 0, 0; */
+ /* 0, 0, 0, 0, 0, 0, 0, 1, 0; */
+ /* 0, 0, 0, 0, 0, 0, 0, 0, 1]; */
+ /* D=[]; */
+ /* */
+ /* */
+ /* sys=ss(A,b,C,D); */
+ /* */
+ /* sysd=c2d(sys,tpred); */
+ /* */
+ /* */
+ /* F=sysd.a; */
+ /* */
+ /* B=sysd.b; */
+ /* */
+ /* H=sysd.c; */
+ /* 'attitudeKalmanfilter:68' udpIndVect=find(updVect); */
+ find(updVect, udpIndVect_data, &udpIndVect_sizes);
+
+ /* 'attitudeKalmanfilter:71' rates_ProcessNoiseMatrix=diag([knownConst(1),knownConst(1),knownConst(2)]); */
+ /* 'attitudeKalmanfilter:72' acc_ProcessNoise=knownConst(3); */
+ /* 'attitudeKalmanfilter:73' mag_ProcessNoise=knownConst(4); */
+ /* 'attitudeKalmanfilter:74' rates_MeasurementNoise=knownConst(6); */
+ /* 'attitudeKalmanfilter:75' acc_MeasurementNoise=knownConst(7); */
+ /* 'attitudeKalmanfilter:76' mag_MeasurementNoise=knownConst(8); */
/* process noise covariance matrix */
- /* 'attitudeKalmanfilter:30' Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:31' zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:32' zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3); */
- /* 'attitudeKalmanfilter:33' zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise]; */
- /* measurement noise covariance matrix */
- /* 'attitudeKalmanfilter:36' R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:37' zeros(3), eye(3)*mag_MeasurementNoise, zeros(3); */
- /* 'attitudeKalmanfilter:38' zeros(3), zeros(3), eye(3)*rates_MeasurementNoise]; */
+ /* 'attitudeKalmanfilter:81' Q = [ rates_ProcessNoiseMatrix, zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:82' zeros(3), eye(3)*mag_ProcessNoise, zeros(3); */
+ /* 'attitudeKalmanfilter:83' zeros(3), zeros(3), eye(3)*acc_ProcessNoise]; */
/* observation matrix */
- /* 'attitudeKalmanfilter:42' H_k=[ eye(3), zeros(3), zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:43' zeros(3), eye(3), zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:44' zeros(3), zeros(3), eye(3), eye(3)]; */
+ /* 'attitudeKalmanfilter:89' H_k_full=[ eye(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:90' zeros(3), eye(3), zeros(3); */
+ /* 'attitudeKalmanfilter:91' zeros(3), zeros(3), eye(3)]; */
/* compute A(t,w) */
/* x_aposteriori_k[10,11,12] should be [p,q,r] */
/* R_temp=[1,-r, q */
/* r, 1, -p */
/* -q, p, 1] */
- /* 'attitudeKalmanfilter:53' R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); */
- /* 'attitudeKalmanfilter:54' dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); */
- /* 'attitudeKalmanfilter:55' -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1]; */
+ /* 'attitudeKalmanfilter:100' R_temp=[1, -dt*x_aposteriori_k(3), dt*x_aposteriori_k(2); */
+ /* 'attitudeKalmanfilter:101' dt*x_aposteriori_k(3), 1, -dt*x_aposteriori_k(1); */
+ /* 'attitudeKalmanfilter:102' -dt*x_aposteriori_k(2), dt*x_aposteriori_k(1), 1]; */
R_temp[0] = 1.0F;
- R_temp[3] = -dt * x_aposteriori_k[11];
- R_temp[6] = dt * x_aposteriori_k[10];
- R_temp[1] = dt * x_aposteriori_k[11];
+ R_temp[3] = -dt * x_aposteriori_k[2];
+ R_temp[6] = dt * x_aposteriori_k[1];
+ R_temp[1] = dt * x_aposteriori_k[2];
R_temp[4] = 1.0F;
- R_temp[7] = -dt * x_aposteriori_k[9];
- R_temp[2] = -dt * x_aposteriori_k[10];
- R_temp[5] = dt * x_aposteriori_k[9];
+ R_temp[7] = -dt * x_aposteriori_k[0];
+ R_temp[2] = -dt * x_aposteriori_k[1];
+ R_temp[5] = dt * x_aposteriori_k[0];
R_temp[8] = 1.0F;
- /* strange, should not be transposed */
- /* 'attitudeKalmanfilter:58' A_pred=[R_temp', zeros(3), zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:59' zeros(3), R_temp', zeros(3), zeros(3); */
- /* 'attitudeKalmanfilter:60' zeros(3), zeros(3), eye(3), zeros(3); */
- /* 'attitudeKalmanfilter:61' zeros(3), zeros(3), zeros(3), eye(3)]; */
+ /* 'attitudeKalmanfilter:106' A_pred=[eye(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:107' zeros(3), R_temp', zeros(3); */
+ /* 'attitudeKalmanfilter:108' zeros(3), zeros(3), R_temp']; */
+ /* 'attitudeKalmanfilter:110' B_pred=[knownConst(9)*dt, 0, 0; */
+ /* 'attitudeKalmanfilter:111' 0, knownConst(10)*dt, 0; */
+ /* 'attitudeKalmanfilter:112' 0, 0, 0; */
+ /* 'attitudeKalmanfilter:113' 0, 0, 0; */
+ /* 'attitudeKalmanfilter:114' 0, 0, 0; */
+ /* 'attitudeKalmanfilter:115' 0, 0, 0; */
+ /* 'attitudeKalmanfilter:116' 0, 0, 0; */
+ /* 'attitudeKalmanfilter:117' 0, 0, 0; */
+ /* 'attitudeKalmanfilter:118' 0, 0, 0]; */
+ /* %prediction step */
+ /* 'attitudeKalmanfilter:121' x_apriori=A_pred*x_aposteriori_k+B_pred*u(1:3); */
eye(dv0);
- eye(dv1);
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[i0 + 12 * i] = R_temp[i + 3 * i0];
+ fv0[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib];
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[i0 + 12 * (i + 3)] = 0.0F;
+ fv0[i0 + 9 * (ib + 3)] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[i0 + 12 * (i + 6)] = 0.0F;
+ fv0[i0 + 9 * (ib + 6)] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[i0 + 12 * (i + 9)] = 0.0F;
+ fv0[(i0 + 9 * ib) + 3] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * i) + 3] = 0.0F;
+ fv0[(i0 + 9 * (ib + 3)) + 3] = R_temp[ib + 3 * i0];
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0];
+ fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+ fv0[(i0 + 9 * ib) + 6] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F;
+ fv0[(i0 + 9 * (ib + 3)) + 6] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ b_knownConst[0] = knownConst[8] * dt;
+ b_knownConst[9] = 0.0F;
+ b_knownConst[18] = 0.0F;
+ b_knownConst[1] = 0.0F;
+ b_knownConst[10] = knownConst[9] * dt;
+ b_knownConst[19] = 0.0F;
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * i) + 6] = 0.0F;
+ fv0[(i0 + 9 * (ib + 6)) + 6] = R_temp[ib + 3 * i0];
}
- }
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
- }
+ b_knownConst[2 + 9 * ib] = 0.0F;
+ b_knownConst[3 + 9 * ib] = 0.0F;
+ b_knownConst[4 + 9 * ib] = 0.0F;
+ b_knownConst[5 + 9 * ib] = 0.0F;
+ b_knownConst[6 + 9 * ib] = 0.0F;
+ b_knownConst[7 + 9 * ib] = 0.0F;
+ b_knownConst[8 + 9 * ib] = 0.0F;
}
- for (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 (ib = 0; ib < 9; ib++) {
+ fv1[ib] = 0.0F;
+ for (i0 = 0; i0 < 9; i0++) {
+ fv1[ib] += fv0[ib + 9 * i0] * x_aposteriori_k[i0];
}
- }
- for (i = 0; i < 3; i++) {
+ c_knownConst[ib] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i];
+ c_knownConst[ib] += b_knownConst[ib + 9 * i0] * u[i0];
}
- }
- /* %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];
- }
+ x_aposteriori[ib] = fv1[ib] + c_knownConst[ib];
}
/* linearization */
- /* 'attitudeKalmanfilter:67' acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2); */
- /* 'attitudeKalmanfilter:68' -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1); */
- /* 'attitudeKalmanfilter:69' dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0]; */
- /* 'attitudeKalmanfilter:71' mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5); */
- /* 'attitudeKalmanfilter:72' -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4); */
- /* 'attitudeKalmanfilter:73' dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0]; */
- /* 'attitudeKalmanfilter:75' A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat'; */
- /* 'attitudeKalmanfilter:76' zeros(3), R_temp', zeros(3), mag_temp_mat'; */
- /* 'attitudeKalmanfilter:77' zeros(3), zeros(3), eye(3), zeros(3); */
- /* 'attitudeKalmanfilter:78' zeros(3), zeros(3), zeros(3), eye(3)]; */
+ /* 'attitudeKalmanfilter:125' temp_mat=[ 0, -dt, dt; */
+ /* 'attitudeKalmanfilter:126' dt, 0, -dt; */
+ /* 'attitudeKalmanfilter:127' -dt, dt, 0]; */
+ R_temp[0] = 0.0F;
+ R_temp[3] = -dt;
+ R_temp[6] = dt;
+ R_temp[1] = dt;
+ R_temp[4] = 0.0F;
+ R_temp[7] = -dt;
+ R_temp[2] = -dt;
+ R_temp[5] = dt;
+ R_temp[8] = 0.0F;
+
+ /* 'attitudeKalmanfilter:129' A_lin=[ eye(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:130' temp_mat, eye(3) , zeros(3); */
+ /* 'attitudeKalmanfilter:131' temp_mat, zeros(3), eye(3)]; */
eye(dv0);
eye(dv1);
- for (i = 0; i < 3; i++) {
+ eye(dv2);
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[i0 + 12 * i] = R_temp[i + 3 * i0];
+ A_lin[i0 + 9 * ib] = (real32_T)dv0[i0 + 3 * ib];
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[i0 + 12 * (i + 3)] = 0.0F;
+ A_lin[i0 + 9 * (ib + 3)] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[i0 + 12 * (i + 6)] = 0.0F;
+ A_lin[i0 + 9 * (ib + 6)] = 0.0F;
}
}
- A_pred[108] = 0.0F;
- A_pred[109] = dt * x_aposteriori_k[2];
- A_pred[110] = -dt * x_aposteriori_k[1];
- A_pred[120] = -dt * x_aposteriori_k[2];
- A_pred[121] = 0.0F;
- A_pred[122] = dt * x_aposteriori_k[0];
- A_pred[132] = dt * x_aposteriori_k[1];
- A_pred[133] = -dt * x_aposteriori_k[0];
- A_pred[134] = 0.0F;
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * i) + 3] = 0.0F;
+ A_lin[(i0 + 9 * ib) + 3] = R_temp[i0 + 3 * ib];
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0];
+ A_lin[(i0 + 9 * (ib + 3)) + 3] = (real32_T)dv1[i0 + 3 * ib];
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+ A_lin[(i0 + 9 * (ib + 6)) + 3] = 0.0F;
}
}
- A_pred[111] = 0.0F;
- A_pred[112] = dt * x_aposteriori_k[5];
- A_pred[113] = -dt * x_aposteriori_k[4];
- A_pred[123] = -dt * x_aposteriori_k[5];
- A_pred[124] = 0.0F;
- A_pred[125] = dt * x_aposteriori_k[3];
- A_pred[135] = dt * x_aposteriori_k[4];
- A_pred[136] = -dt * x_aposteriori_k[3];
- A_pred[137] = 0.0F;
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * i) + 6] = 0.0F;
+ A_lin[(i0 + 9 * ib) + 6] = R_temp[i0 + 3 * ib];
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ A_lin[(i0 + 9 * (ib + 3)) + 6] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i];
+ A_lin[(i0 + 9 * (ib + 6)) + 6] = (real32_T)dv2[i0 + 3 * ib];
}
}
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
- }
- }
-
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * i) + 9] = 0.0F;
- }
- }
-
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
- }
- }
-
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
- }
- }
-
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i];
- }
- }
-
- /* 'attitudeKalmanfilter:81' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- b_A_pred[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- b_A_pred[i + 12 * i0] += A_pred[i + 12 * i1] * P_aposteriori_k[i1 + 12 *
+ /* 'attitudeKalmanfilter:134' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
+ x_n_b[0] = knownConst[0];
+ x_n_b[1] = knownConst[0];
+ x_n_b[2] = knownConst[1];
+ diag(x_n_b, R_temp);
+ for (ib = 0; ib < 9; ib++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ K_k_data[ib + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ K_k_data[ib + 9 * i0] += A_lin[ib + 9 * i1] * P_aposteriori_k[i1 + 9 *
i0];
}
}
- for (i0 = 0; i0 < 12; i0++) {
- c_A_pred[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- c_A_pred[i + 12 * i0] += b_A_pred[i + 12 * i1] * A_pred[i0 + 12 * i1];
+ for (i0 = 0; i0 < 9; i0++) {
+ b_A_lin[ib + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ b_A_lin[ib + 9 * i0] += K_k_data[ib + 9 * i1] * A_lin[i0 + 9 * i1];
}
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[0];
+ K_k_data[i0 + 9 * ib] = R_temp[i0 + 3 * ib];
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[i0 + 12 * (i + 3)] = 0.0F;
+ K_k_data[i0 + 9 * (ib + 3)] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[i0 + 12 * (i + 6)] = 0.0F;
+ K_k_data[i0 + 9 * (ib + 6)] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[i0 + 12 * (i + 9)] = 0.0F;
+ K_k_data[(i0 + 9 * ib) + 3] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[(i0 + 12 * i) + 3] = 0.0F;
+ K_k_data[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] *
+ knownConst[3];
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[(i0 + 12 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] *
- knownConst[1];
+ K_k_data[(i0 + 9 * (ib + 6)) + 3] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+ K_k_data[(i0 + 9 * ib) + 6] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F;
+ K_k_data[(i0 + 9 * (ib + 3)) + 6] = 0.0F;
}
}
- for (i = 0; i < 3; i++) {
+ for (ib = 0; ib < 3; ib++) {
for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[(i0 + 12 * i) + 6] = 0.0F;
+ K_k_data[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] *
+ knownConst[2];
}
}
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ for (ib = 0; ib < 9; ib++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ P_apriori[i0 + 9 * ib] = b_A_lin[i0 + 9 * ib] + K_k_data[i0 + 9 * ib];
}
}
- 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];
+ /* 'attitudeKalmanfilter:137' if ~isempty(udpIndVect)==1 */
+ if (!(udpIndVect_sizes == 0) == 1) {
+ /* 'attitudeKalmanfilter:138' H_k= H_k_full(udpIndVect,:); */
+ for (ib = 0; ib < 9; ib++) {
+ ia = udpIndVect_sizes - 1;
+ for (i0 = 0; i0 <= ia; i0++) {
+ H_k_data[i0 + udpIndVect_sizes * ib] = H_k_full[((int32_T)
+ udpIndVect_data[i0] + 9 * ib) - 1];
+ }
}
- }
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
- }
- }
+ /* %update step */
+ /* 'attitudeKalmanfilter:140' accUpt=1; */
+ accUpt = 1;
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[(i0 + 12 * i) + 9] = 0.0F;
- }
- }
+ /* 'attitudeKalmanfilter:141' magUpt=1; */
+ magUpt = 1;
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
- }
- }
+ /* 'attitudeKalmanfilter:142' y_k=z_k-H_k*x_apriori; */
+ ia = udpIndVect_sizes - 1;
+ for (ib = 0; ib <= ia; ib++) {
+ y = 0.0F;
+ for (i0 = 0; i0 < 9; i0++) {
+ y += (real32_T)H_k_data[ib + udpIndVect_sizes * i0] * x_aposteriori[i0];
+ }
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- b_A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+ y_k_data[ib] = z_k_data[ib] - y;
}
- }
- 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];
- }
- }
+ /* 'attitudeKalmanfilter:143' if updVect(4)==1 */
+ if (updVect[3] == 1) {
+ /* 'attitudeKalmanfilter:144' if (abs(norm(z_k(4:6))-knownConst(12))>knownConst(14)) */
+ for (ib = 0; ib < 3; ib++) {
+ x_n_b[ib] = z_k_data[ib + 3];
+ }
- 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];
+ if ((real32_T)fabs(norm(x_n_b) - knownConst[11]) > knownConst[13]) {
+ /* 'attitudeKalmanfilter:145' accUpt=10000; */
+ accUpt = 10000;
+ }
}
- }
- /* %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];
+ /* 'attitudeKalmanfilter:149' if updVect(7)==1 */
+ if (updVect[6] == 1) {
+ /* 'attitudeKalmanfilter:150' if (abs(norm(z_k(7:9))-knownConst(13))>knownConst(15)) */
+ for (ib = 0; ib < 3; ib++) {
+ x_n_b[ib] = z_k_data[ib + 6];
+ }
+
+ if ((real32_T)fabs(norm(x_n_b) - knownConst[12]) > knownConst[14]) {
+ /* 'attitudeKalmanfilter:152' magUpt=10000; */
+ magUpt = 10000;
}
}
- }
- 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];
+ /* measurement noise covariance matrix */
+ /* 'attitudeKalmanfilter:157' R = [ eye(3)*rates_MeasurementNoise, zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:158' zeros(3), eye(3)*acc_MeasurementNoise*accUpt, zeros(3); */
+ /* 'attitudeKalmanfilter:159' zeros(3), zeros(3), eye(3)*mag_MeasurementNoise*magUpt]; */
+ /* 'attitudeKalmanfilter:161' S_k=H_k*P_apriori*H_k'+R(udpIndVect,udpIndVect); */
+ /* 'attitudeKalmanfilter:162' K_k=(P_apriori*H_k'/(S_k)); */
+ P_apriori_sizes[0] = 9;
+ P_apriori_sizes[1] = udpIndVect_sizes;
+ for (ib = 0; ib < 9; ib++) {
+ ia = udpIndVect_sizes - 1;
+ for (i0 = 0; i0 <= ia; i0++) {
+ b_A_lin[ib + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ b_A_lin[ib + 9 * i0] += P_apriori[ib + 9 * i1] * (real32_T)H_k_data[i0
+ + udpIndVect_sizes * i1];
+ }
}
}
- 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];
+ ia = udpIndVect_sizes - 1;
+ for (ib = 0; ib <= ia; ib++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ K_k_data[ib + udpIndVect_sizes * i0] = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ K_k_data[ib + udpIndVect_sizes * i0] += (real32_T)H_k_data[ib +
+ udpIndVect_sizes * i1] * P_apriori[i1 + 9 * i0];
+ }
}
}
- }
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv1[i0 + 9 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[4];
+ for (ib = 0; ib < 3; ib++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv0[i0 + 9 * ib] = (real32_T)iv0[i0 + 3 * ib] * knownConst[5];
+ }
}
- }
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv1[i0 + 9 * (i + 3)] = 0.0F;
+ for (ib = 0; ib < 3; ib++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv0[i0 + 9 * (ib + 3)] = 0.0F;
+ }
}
- }
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv1[i0 + 9 * (i + 6)] = 0.0F;
+ for (ib = 0; ib < 3; ib++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv0[i0 + 9 * (ib + 6)] = 0.0F;
+ }
}
- }
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv1[(i0 + 9 * i) + 3] = 0.0F;
+ for (ib = 0; ib < 3; ib++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv0[(i0 + 9 * ib) + 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 (ib = 0; ib < 3; ib++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv0[(i0 + 9 * (ib + 3)) + 3] = (real32_T)iv0[i0 + 3 * ib] * knownConst[6]
+ * (real32_T)accUpt;
+ }
}
- }
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv1[(i0 + 9 * (i + 6)) + 3] = 0.0F;
+ for (ib = 0; ib < 3; ib++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv0[(i0 + 9 * (ib + 6)) + 3] = 0.0F;
+ }
}
- }
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv1[(i0 + 9 * i) + 6] = 0.0F;
+ for (ib = 0; ib < 3; ib++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv0[(i0 + 9 * ib) + 6] = 0.0F;
+ }
}
- }
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- fv1[(i0 + 9 * (i + 3)) + 6] = 0.0F;
+ for (ib = 0; ib < 3; ib++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv0[(i0 + 9 * (ib + 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 (ib = 0; ib < 3; ib++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv0[(i0 + 9 * (ib + 6)) + 6] = (real32_T)iv0[i0 + 3 * ib] * knownConst[7]
+ * (real32_T)magUpt;
+ }
}
- }
- for (i = 0; i < 9; i++) {
- for (i0 = 0; i0 < 9; i0++) {
- fv2[i0 + 9 * i] = fv0[i0 + 9 * i] + fv1[i0 + 9 * i];
+ H_k_sizes[0] = udpIndVect_sizes;
+ H_k_sizes[1] = udpIndVect_sizes;
+ ia = udpIndVect_sizes - 1;
+ for (ib = 0; ib <= ia; ib++) {
+ accUpt = udpIndVect_sizes - 1;
+ for (i0 = 0; i0 <= accUpt; i0++) {
+ y = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ y += K_k_data[ib + udpIndVect_sizes * i1] * (real32_T)H_k_data[i0 +
+ udpIndVect_sizes * i1];
+ }
+
+ A_lin[ib + H_k_sizes[0] * i0] = y + fv0[((int32_T)udpIndVect_data[ib] +
+ 9 * ((int32_T)udpIndVect_data[i0] - 1)) - 1];
+ }
}
- }
- mrdivide(b_P_apriori, fv2, K_k);
+ mrdivide(b_A_lin, P_apriori_sizes, A_lin, H_k_sizes, K_k_data, K_k_sizes);
- /* '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];
- }
+ /* 'attitudeKalmanfilter:165' x_aposteriori=x_apriori+K_k*y_k; */
+ if ((K_k_sizes[1] == 1) || (udpIndVect_sizes == 1)) {
+ for (ib = 0; ib < 9; ib++) {
+ b_y[ib] = 0.0F;
+ ia = udpIndVect_sizes - 1;
+ for (i0 = 0; i0 <= ia; i0++) {
+ b_y[ib] += K_k_data[ib + K_k_sizes[0] * i0] * y_k_data[i0];
+ }
+ }
+ } else {
+ for (accUpt = 0; accUpt < 9; accUpt++) {
+ b_y[accUpt] = 0.0F;
+ }
- R_temp[i] = z_k[i] - B;
- }
+ magUpt = -1;
+ for (ib = 0; ib + 1 <= K_k_sizes[1]; ib++) {
+ if ((real_T)y_k_data[ib] != 0.0) {
+ ia = magUpt;
+ for (accUpt = 0; accUpt < 9; accUpt++) {
+ ia++;
+ b_y[accUpt] += y_k_data[ib] * K_k_data[ia];
+ }
+ }
+
+ magUpt += 9;
+ }
+ }
- for (i = 0; i < 12; i++) {
- B = 0.0F;
- for (i0 = 0; i0 < 9; i0++) {
- B += K_k[i + 12 * i0] * R_temp[i0];
+ for (ib = 0; ib < 9; ib++) {
+ x_aposteriori[ib] += b_y[ib];
}
- x_aposteriori[i] = x_apriori[i] + B;
- }
+ /* 'attitudeKalmanfilter:166' P_aposteriori=(eye(9)-K_k*H_k)*P_apriori; */
+ b_eye(dv3);
+ for (ib = 0; ib < 9; ib++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ y = 0.0F;
+ ia = K_k_sizes[1] - 1;
+ for (i1 = 0; i1 <= ia; i1++) {
+ y += K_k_data[ib + K_k_sizes[0] * i1] * (real32_T)H_k_data[i1 +
+ udpIndVect_sizes * i0];
+ }
- /* '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];
+ fv0[ib + 9 * i0] = (real32_T)dv3[ib + 9 * i0] - y;
}
-
- 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];
+ for (ib = 0; ib < 9; ib++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ P_aposteriori[ib + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ P_aposteriori[ib + 9 * i0] += fv0[ib + 9 * i1] * P_apriori[i1 + 9 * i0];
+ }
}
}
+ } else {
+ /* 'attitudeKalmanfilter:167' else */
+ /* 'attitudeKalmanfilter:168' x_aposteriori=x_apriori; */
+ /* 'attitudeKalmanfilter:169' P_aposteriori=P_apriori; */
+ memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 81U * sizeof
+ (real32_T));
+ }
+
+ /* %% euler anglels extraction */
+ /* 'attitudeKalmanfilter:175' z_n_b = -x_aposteriori(4:6)./norm(x_aposteriori(4:6)); */
+ y = norm(*(real32_T (*)[3])&x_aposteriori[3]);
+
+ /* 'attitudeKalmanfilter:176' m_n_b = x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
+ c_y = norm(*(real32_T (*)[3])&x_aposteriori[6]);
+
+ /* 'attitudeKalmanfilter:178' y_n_b=cross(z_n_b,m_n_b); */
+ for (accUpt = 0; accUpt < 3; accUpt++) {
+ z_n_b[accUpt] = -x_aposteriori[accUpt + 3] / y;
+ x_n_b[accUpt] = x_aposteriori[accUpt + 6] / c_y;
}
- /* %Rotation matrix generation */
- /* 'attitudeKalmanfilter:97' earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); */
- B = norm(*(real32_T (*)[3])&x_aposteriori[0]);
+ cross(z_n_b, x_n_b, y_n_b);
- /* 'attitudeKalmanfilter:98' earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); */
- b_B = norm(*(real32_T (*)[3])&x_aposteriori[3]);
- for (i = 0; i < 3; i++) {
- earth_z[i] = x_aposteriori[i] / B;
- y[i] = x_aposteriori[i + 3] / b_B;
+ /* 'attitudeKalmanfilter:179' y_n_b=y_n_b./norm(y_n_b); */
+ y = norm(y_n_b);
+ for (ib = 0; ib < 3; ib++) {
+ y_n_b[ib] /= y;
}
- 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:181' x_n_b=(cross(y_n_b,z_n_b)); */
+ cross(y_n_b, z_n_b, x_n_b);
- /* 'attitudeKalmanfilter:99' earth_y=cross(earth_x,earth_z); */
- /* 'attitudeKalmanfilter:101' Rot_matrix=[earth_x,earth_y,earth_z]; */
- y[0] = earth_x[1] * earth_z[2] - earth_x[2] * earth_z[1];
- y[1] = earth_x[2] * earth_z[0] - earth_x[0] * earth_z[2];
- y[2] = earth_x[0] * earth_z[1] - earth_x[1] * earth_z[0];
- for (i = 0; i < 3; i++) {
- Rot_matrix[i] = earth_x[i];
- Rot_matrix[3 + i] = y[i];
- Rot_matrix[6 + i] = earth_z[i];
+ /* 'attitudeKalmanfilter:182' x_n_b=x_n_b./norm(x_n_b); */
+ y = norm(x_n_b);
+ for (ib = 0; ib < 3; ib++) {
+ /* 'attitudeKalmanfilter:188' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
+ Rot_matrix[ib] = x_n_b[ib] / y;
+ Rot_matrix[3 + ib] = y_n_b[ib];
+ Rot_matrix[6 + ib] = z_n_b[ib];
}
+
+ /* 'attitudeKalmanfilter:192' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
+ /* 'attitudeKalmanfilter:193' theta=-asin(Rot_matrix(1,3)); */
+ /* 'attitudeKalmanfilter:194' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
+ /* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */
+ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
+ eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
+ eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
}
/* End of code generation (attitudeKalmanfilter.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index 7aa3d048b..8207aa5c5 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:22 2012
*
*/
@@ -14,6 +14,8 @@ #include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
@@ -27,6 +29,6 @@ /* Variable Definitions */
/* Function Declarations */
-extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
+extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[9], real32_T P_aposteriori[81]);
#endif
/* End of code generation (attitudeKalmanfilter.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c index 2800191c3..abf1519a6 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter_initialize'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:23 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index d2d97bb3b..20186f183 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter_initialize'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:24 2012
*
*/
@@ -14,6 +14,8 @@ #include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp deleted file mode 100755 index e69de29bb..000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp +++ /dev/null diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat deleted file mode 100755 index 76fb78ca7..000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat +++ /dev/null @@ -1,11 +0,0 @@ -@echo off
-call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64
-
-cd .
-nmake -f attitudeKalmanfilter_rtw.mk GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
-@if errorlevel 1 goto error_exit
-exit /B 0
-
-:error_exit
-echo The make command returned an error of %errorlevel%
-An_error_occurred_during_the_call_to_make
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk deleted file mode 100755 index b2123704b..000000000 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk +++ /dev/null @@ -1,328 +0,0 @@ -# Copyright 1994-2010 The MathWorks, Inc.
-#
-# File : xrt_vcx64.tmf $Revision: 1.1.6.1 $
-#
-# Abstract:
-# Code generation template makefile for building a Windows-based,
-# stand-alone real-time version of MATLAB-code using generated C/C++
-# code and the
-# Microsoft Visual C/C++ compiler version 8, 9, 10 for x64
-#
-# Note that this template is automatically customized by the
-# code generation build procedure to create "<model>.mk"
-#
-# The following defines can be used to modify the behavior of the
-# build:
-#
-# OPT_OPTS - Optimization option. See DEFAULT_OPT_OPTS in
-# vctools.mak for default.
-# OPTS - User specific options.
-# CPP_OPTS - C++ compiler options.
-# USER_SRCS - Additional user sources, such as files needed by
-# S-functions.
-# USER_INCLUDES - Additional include paths
-# (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2")
-#
-# To enable debugging:
-# set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc)
-# modify tmf LDFLAGS to include /DEBUG
-#
-
-#------------------------ Macros read by make_rtw -----------------------------
-#
-# The following macros are read by the code generation build procedure:
-#
-# MAKECMD - This is the command used to invoke the make utility
-# HOST - What platform this template makefile is targeted for
-# (i.e. PC or UNIX)
-# BUILD - Invoke make from the code generation build procedure
-# (yes/no)?
-# SYS_TARGET_FILE - Name of system target file.
-
-MAKECMD = nmake
-HOST = PC
-BUILD = yes
-SYS_TARGET_FILE = ert.tlc
-BUILD_SUCCESS = ^#^#^# Created
-COMPILER_TOOL_CHAIN = vcx64
-
-#---------------------- Tokens expanded by make_rtw ---------------------------
-#
-# The following tokens, when wrapped with "|>" and "<|" are expanded by the
-# code generation build procedure.
-#
-# MODEL_NAME - Name of the Simulink block diagram
-# MODEL_MODULES - Any additional generated source modules
-# MAKEFILE_NAME - Name of makefile created from template makefile <model>.mk
-# MATLAB_ROOT - Path to where MATLAB is installed.
-# MATLAB_BIN - Path to MATLAB executable.
-# S_FUNCTIONS - List of S-functions.
-# S_FUNCTIONS_LIB - List of S-functions libraries to link.
-# SOLVER - Solver source file name
-# NUMST - Number of sample times
-# TID01EQ - yes (1) or no (0): Are sampling rates of continuous task
-# (tid=0) and 1st discrete task equal.
-# NCSTATES - Number of continuous states
-# BUILDARGS - Options passed in at the command line.
-# MULTITASKING - yes (1) or no (0): Is solver mode multitasking
-# EXT_MODE - yes (1) or no (0): Build for external mode
-# TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode
-# testing.
-# EXTMODE_TRANSPORT - Index of transport mechanism (e.g. tcpip, serial) for extmode
-# EXTMODE_STATIC - yes (1) or no (0): Use static instead of dynamic mem alloc.
-# EXTMODE_STATIC_SIZE - Size of static memory allocation buffer.
-
-MODEL = attitudeKalmanfilter
-MODULES = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c
-MAKEFILE = attitudeKalmanfilter_rtw.mk
-MATLAB_ROOT = C:\Program Files\MATLAB\R2011a
-ALT_MATLAB_ROOT = C:\PROGRA~1\MATLAB\R2011a
-MATLAB_BIN = C:\Program Files\MATLAB\R2011a\bin
-ALT_MATLAB_BIN = C:\PROGRA~1\MATLAB\R2011a\bin
-S_FUNCTIONS =
-S_FUNCTIONS_LIB =
-SOLVER =
-NUMST = 1
-TID01EQ = 0
-NCSTATES = 0
-BUILDARGS = GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
-MULTITASKING = 0
-EXT_MODE = 0
-TMW_EXTMODE_TESTING = 0
-EXTMODE_TRANSPORT = 0
-EXTMODE_STATIC = 0
-EXTMODE_STATIC_SIZE = 1000000
-
-MODELREFS =
-SHARED_SRC =
-SHARED_SRC_DIR =
-SHARED_BIN_DIR =
-SHARED_LIB =
-TARGET_LANG_EXT = c
-OPTIMIZATION_FLAGS = /O2 /Oy-
-ADDITIONAL_LDFLAGS =
-
-#--------------------------- Model and reference models -----------------------
-MODELLIB = attitudeKalmanfilter.lib
-MODELREF_LINK_LIBS =
-MODELREF_LINK_RSPFILE = attitudeKalmanfilter_ref.rsp
-MODELREF_INC_PATH =
-RELATIVE_PATH_TO_ANCHOR = F:\CODEGE~1
-MODELREF_TARGET_TYPE = RTW
-
-!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)"
-MATLAB_ROOT = $(ALT_MATLAB_ROOT)
-!endif
-!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)"
-MATLAB_BIN = $(ALT_MATLAB_BIN)
-!endif
-
-#--------------------------- Tool Specifications ------------------------------
-
-CPU = AMD64
-!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak
-APPVER = 5.02
-
-PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl
-#------------------------------ Include/Lib Path ------------------------------
-
-MATLAB_INCLUDES = $(MATLAB_ROOT)\simulink\include
-MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include
-MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src
-MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common
-
-# Additional file include paths
-
-MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter
-MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter
-
-INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH)
-
-!if "$(SHARED_SRC_DIR)" != ""
-INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR)
-!endif
-
-#------------------------ External mode ---------------------------------------
-# Uncomment -DVERBOSE to have information printed to stdout
-# To add a new transport layer, see the comments in
-# <matlabroot>/toolbox/simulink/simulink/extmode_transports.m
-!if $(EXT_MODE) == 1
-EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE
-!if $(EXTMODE_TRANSPORT) == 0 #tcpip
-EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c
-EXT_LIB = wsock32.lib
-!endif
-!if $(EXTMODE_TRANSPORT) == 1 #serial_win32
-EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c
-EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c
-EXT_LIB =
-!endif
-!if $(TMW_EXTMODE_TESTING) == 1
-EXT_SRC = $(EXT_SRC) ext_test.c
-EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING
-!endif
-!if $(EXTMODE_STATIC) == 1
-EXT_SRC = $(EXT_SRC) mem_mgr.c
-EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE)
-!endif
-!else
-EXT_SRC =
-EXT_CC_OPTS =
-EXT_LIB =
-!endif
-
-#------------------------ rtModel ----------------------------------------------
-
-RTM_CC_OPTS = -DUSE_RTMODEL
-
-#----------------- Compiler and Linker Options --------------------------------
-
-# Optimization Options
-# Set OPT_OPTS=-Zi for debugging (may depend on compiler version)
-OPT_OPTS = $(DEFAULT_OPT_OPTS)
-
-# General User Options
-OPTS =
-
-!if "$(OPTIMIZATION_FLAGS)" != ""
-CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS)
-!else
-CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS)
-!endif
-
-CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \
- -DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \
- -DMT=$(MULTITASKING) -DHAVESTDIO
-
-# Uncomment this line to move warning level to W4
-# cflags = $(cflags:W3=W4)
-CFLAGS = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
- $(cflags) $(cvarsmt) /wd4996
-CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
- $(cflags) $(cvarsmt) /wd4996 /EHsc-
-LDFLAGS = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS)
-
-# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib
-
-#----------------------------- Source Files -----------------------------------
-
-
-#Standalone executable
-!if "$(MODELREF_TARGET_TYPE)" == "NONE"
-PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe
-REQ_SRCS = $(MODULES) $(EXT_SRC)
-
-#Model Reference Target
-!else
-PRODUCT = $(MODELLIB)
-REQ_SRCS = $(MODULES) $(EXT_SRC)
-!endif
-
-USER_SRCS =
-
-SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS)
-OBJS_CPP_UPPER = $(SRCS:.CPP=.obj)
-OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj)
-OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj)
-OBJS = $(OBJS_C_UPPER:.c=.obj)
-SHARED_OBJS = $(SHARED_SRC:.c=.obj)
-# ------------------------- Additional Libraries ------------------------------
-
-LIBS =
-
-
-LIBS = $(LIBS)
-
-# ---------------------------- Linker Script ----------------------------------
-
-CMD_FILE = $(MODEL).lnk
-GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl
-
-#--------------------------------- Rules --------------------------------------
-all: set_environment_variables $(PRODUCT)
-
-!if "$(MODELREF_TARGET_TYPE)" == "NONE"
-#--- Stand-alone model ---
-$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS)
- @cmd /C "echo ### Linking ..."
- $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
- $(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@
- @del $(CMD_FILE)
- @cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe"
-!else
-#--- Model reference code generation Target ---
-$(PRODUCT) : $(OBJS) $(SHARED_LIB)
- @cmd /C "echo ### Linking ..."
- $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
- $(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB)
- @cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)"
-!endif
-
-{$(MATLAB_ROOT)\rtw\c\src}.c.obj :
- @cmd /C "echo ### Compiling $<"
- $(CC) $(CFLAGS) $<
-
-{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj :
- @cmd /C "echo ### Compiling $<"
- $(CC) $(CFLAGS) $<
-
-{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj :
- @cmd /C "echo ### Compiling $<"
- $(CC) $(CFLAGS) $<
-
-{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj :
- @cmd /C "echo ### Compiling $<"
- $(CC) $(CFLAGS) $<
-
-{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj :
- @cmd /C "echo ### Compiling $<"
- $(CC) $(CFLAGS) $<
-
-# Additional sources
-
-
-
-
-
-# Put these rule last, otherwise nmake will check toolboxes first
-
-{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj :
- @cmd /C "echo ### Compiling $<"
- $(CC) $(CFLAGS) $<
-
-{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj :
- @cmd /C "echo ### Compiling $<"
- $(CC) $(CPPFLAGS) $<
-
-.c.obj :
- @cmd /C "echo ### Compiling $<"
- $(CC) $(CFLAGS) $<
-
-.cpp.obj :
- @cmd /C "echo ### Compiling $<"
- $(CC) $(CPPFLAGS) $<
-
-!if "$(SHARED_LIB)" != ""
-$(SHARED_LIB) : $(SHARED_SRC)
- @cmd /C "echo ### Creating $@"
- @$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<<
-$?
-<<
- @$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS)
- @cmd /C "echo ### $@ Created"
-!endif
-
-
-set_environment_variables:
- @set INCLUDE=$(INCLUDE)
- @set LIB=$(LIB)
-
-# Libraries:
-
-
-
-
-
-#----------------------------- Dependencies -----------------------------------
-
-$(OBJS) : $(MAKEFILE) rtw_proj.tmw
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c index eab8c7d6e..458ddb9eb 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter_terminate'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:24 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index fafd866e4..a939eee20 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter_terminate'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:24 2012
*
*/
@@ -14,6 +14,8 @@ #include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h index 05f4664cd..599a16884 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:22 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/buildInfo.mat b/apps/attitude_estimator_ekf/codegen/buildInfo.mat Binary files differdeleted file mode 100755 index b811d0039..000000000 --- a/apps/attitude_estimator_ekf/codegen/buildInfo.mat +++ /dev/null diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c new file mode 100755 index 000000000..49f655ece --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -0,0 +1,37 @@ +/*
+ * cross.c
+ *
+ * Code generation for function 'cross'
+ *
+ * C source code generated on: Mon Sep 17 20:13:23 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "cross.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
+{
+ c[0] = a[1] * b[2] - a[2] * b[1];
+ c[1] = a[2] * b[0] - a[0] * b[2];
+ c[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+/* End of code generation (cross.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h new file mode 100755 index 000000000..844d8138f --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -0,0 +1,34 @@ +/*
+ * cross.h
+ *
+ * Code generation for function 'cross'
+ *
+ * C source code generated on: Mon Sep 17 20:13:23 2012
+ *
+ */
+
+#ifndef __CROSS_H__
+#define __CROSS_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
+#endif
+/* End of code generation (cross.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c new file mode 100755 index 000000000..e54d52a38 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/diag.c @@ -0,0 +1,42 @@ +/*
+ * diag.c
+ *
+ * Code generation for function 'diag'
+ *
+ * C source code generated on: Mon Sep 17 20:13:22 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "diag.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void diag(const real32_T v[3], real32_T d[9])
+{
+ int32_T j;
+ for (j = 0; j < 9; j++) {
+ d[j] = 0.0F;
+ }
+
+ for (j = 0; j < 3; j++) {
+ d[j + 3 * j] = v[j];
+ }
+}
+
+/* End of code generation (diag.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h new file mode 100755 index 000000000..a4a2a4c82 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -0,0 +1,34 @@ +/*
+ * diag.h
+ *
+ * Code generation for function 'diag'
+ *
+ * C source code generated on: Mon Sep 17 20:13:23 2012
+ *
+ */
+
+#ifndef __DIAG_H__
+#define __DIAG_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void diag(const real32_T v[3], real32_T d[9]);
+#endif
+/* End of code generation (diag.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c index e4655839c..aece401c2 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -3,7 +3,7 @@ *
* Code generation for function 'eye'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:23 2012
*
*/
@@ -27,12 +27,12 @@ /*
*
*/
-void b_eye(real_T I[144])
+void b_eye(real_T I[81])
{
int32_T i;
- memset((void *)&I[0], 0, 144U * sizeof(real_T));
- for (i = 0; i < 12; i++) {
- I[i + 12 * i] = 1.0;
+ memset((void *)&I[0], 0, 81U * sizeof(real_T));
+ for (i = 0; i < 9; i++) {
+ I[i + 9 * i] = 1.0;
}
}
diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h index e8881747f..e715ae1c3 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -3,7 +3,7 @@ *
* Code generation for function 'eye'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:23 2012
*
*/
@@ -14,6 +14,8 @@ #include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
@@ -27,7 +29,7 @@ /* Variable Definitions */
/* Function Declarations */
-extern void b_eye(real_T I[144]);
+extern void b_eye(real_T I[81]);
extern void eye(real_T I[9]);
#endif
/* End of code generation (eye.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/find.c b/apps/attitude_estimator_ekf/codegen/find.c new file mode 100755 index 000000000..532ba4397 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/find.c @@ -0,0 +1,77 @@ +/*
+ * find.c
+ *
+ * Code generation for function 'find'
+ *
+ * C source code generated on: Mon Sep 17 20:13:22 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "find.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1])
+{
+ int32_T idx;
+ int32_T ii;
+ boolean_T exitg1;
+ boolean_T guard1 = FALSE;
+ int32_T i2;
+ int8_T b_i_data[9];
+ idx = 0;
+ i_sizes[0] = 9;
+ ii = 1;
+ exitg1 = 0U;
+ while ((exitg1 == 0U) && (ii <= 9)) {
+ guard1 = FALSE;
+ if (x[ii - 1] != 0) {
+ idx++;
+ i_data[idx - 1] = (real_T)ii;
+ if (idx >= 9) {
+ exitg1 = 1U;
+ } else {
+ guard1 = TRUE;
+ }
+ } else {
+ guard1 = TRUE;
+ }
+
+ if (guard1 == TRUE) {
+ ii++;
+ }
+ }
+
+ if (1 > idx) {
+ idx = 0;
+ }
+
+ ii = idx - 1;
+ for (i2 = 0; i2 <= ii; i2++) {
+ b_i_data[i2] = (int8_T)i_data[i2];
+ }
+
+ i_sizes[0] = idx;
+ ii = idx - 1;
+ for (i2 = 0; i2 <= ii; i2++) {
+ i_data[i2] = (real_T)b_i_data[i2];
+ }
+}
+
+/* End of code generation (find.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/find.h b/apps/attitude_estimator_ekf/codegen/find.h new file mode 100755 index 000000000..ceb90b6cf --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/find.h @@ -0,0 +1,34 @@ +/*
+ * find.h
+ *
+ * Code generation for function 'find'
+ *
+ * C source code generated on: Mon Sep 17 20:13:22 2012
+ *
+ */
+
+#ifndef __FIND_H__
+#define __FIND_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]);
+#endif
+/* End of code generation (find.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c index cb81b5328..2fcaf8cb6 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -3,7 +3,7 @@ *
* Code generation for function 'mrdivide'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:23 2012
*
*/
@@ -21,136 +21,719 @@ /* Variable Definitions */
/* Function Declarations */
+static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x);
+static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2],
+ real32_T B_data[81], int32_T B_sizes[2]);
+static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data
+ [81], int32_T x_sizes[2], int32_T ix0);
+static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2],
+ real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes
+ [2]);
+static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T
+ x_sizes[2], int32_T ix0);
/* Function Definitions */
/*
*
*/
-void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
+static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x)
{
- int32_T jy;
+ return 0.0F;
+}
+
+/*
+ *
+ */
+static void eml_lusolve(const real32_T A_data[81], const int32_T A_sizes[2],
+ real32_T B_data[81], int32_T B_sizes[2])
+{
+ int32_T loop_ub;
int32_T iy;
- real32_T b_A[81];
- int8_T ipiv[9];
+ real32_T b_A_data[81];
+ int32_T jA;
+ int32_T tmp_data[9];
+ int32_T ipiv_data[9];
+ int32_T ldap1;
int32_T j;
int32_T mmj;
int32_T jj;
- int32_T jp1j;
- int32_T c;
int32_T ix;
real32_T temp;
int32_T k;
real32_T s;
- int32_T loop_ub;
- real32_T Y[108];
- for (jy = 0; jy < 9; jy++) {
- for (iy = 0; iy < 9; iy++) {
- b_A[iy + 9 * jy] = B[jy + 9 * iy];
+ int32_T jrow;
+ int32_T b_loop_ub;
+ int32_T c;
+ loop_ub = A_sizes[0] * A_sizes[1] - 1;
+ for (iy = 0; iy <= loop_ub; iy++) {
+ b_A_data[iy] = A_data[iy];
+ }
+
+ iy = A_sizes[1];
+ jA = A_sizes[1];
+ jA = iy <= jA ? iy : jA;
+ if (jA < 1) {
+ } else {
+ loop_ub = jA - 1;
+ for (iy = 0; iy <= loop_ub; iy++) {
+ tmp_data[iy] = 1 + iy;
}
- ipiv[jy] = (int8_T)(1 + jy);
+ loop_ub = jA - 1;
+ for (iy = 0; iy <= loop_ub; iy++) {
+ ipiv_data[iy] = tmp_data[iy];
+ }
}
- for (j = 0; j < 8; j++) {
- mmj = -j;
- jj = j * 10;
- jp1j = jj + 1;
- c = mmj + 9;
- jy = 0;
- ix = jj;
- temp = fabsf(b_A[jj]);
- for (k = 2; k <= c; k++) {
- ix++;
- s = fabsf(b_A[ix]);
- if (s > temp) {
- jy = k - 1;
- temp = s;
+ ldap1 = A_sizes[1] + 1;
+ iy = A_sizes[1] - 1;
+ jA = A_sizes[1];
+ loop_ub = iy <= jA ? iy : jA;
+ for (j = 1; j <= loop_ub; j++) {
+ mmj = (A_sizes[1] - j) + 1;
+ jj = (j - 1) * ldap1;
+ if (mmj < 1) {
+ jA = -1;
+ } else {
+ jA = 0;
+ if (mmj > 1) {
+ ix = jj;
+ temp = (real32_T)fabs(b_A_data[jj]);
+ for (k = 1; k + 1 <= mmj; k++) {
+ ix++;
+ s = (real32_T)fabs(b_A_data[ix]);
+ if (s > temp) {
+ jA = k;
+ temp = s;
+ }
+ }
}
}
- if ((real_T)b_A[jj + jy] != 0.0) {
- if (jy != 0) {
- ipiv[j] = (int8_T)((j + jy) + 1);
- ix = j;
- iy = j + jy;
- for (k = 0; k < 9; k++) {
- temp = b_A[ix];
- b_A[ix] = b_A[iy];
- b_A[iy] = temp;
- ix += 9;
- iy += 9;
+ if ((real_T)b_A_data[jj + jA] != 0.0) {
+ if (jA != 0) {
+ ipiv_data[j - 1] = j + jA;
+ jrow = j - 1;
+ iy = jrow + jA;
+ for (k = 1; k <= A_sizes[1]; k++) {
+ temp = b_A_data[jrow];
+ b_A_data[jrow] = b_A_data[iy];
+ b_A_data[iy] = temp;
+ jrow += A_sizes[1];
+ iy += A_sizes[1];
}
}
- loop_ub = (jp1j + mmj) + 8;
- for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
- b_A[iy] /= b_A[jj];
+ b_loop_ub = jj + mmj;
+ for (jA = jj + 1; jA + 1 <= b_loop_ub; jA++) {
+ b_A_data[jA] /= b_A_data[jj];
}
}
- c = 8 - j;
- jy = jj + 9;
- for (iy = 1; iy <= c; iy++) {
- if ((real_T)b_A[jy] != 0.0) {
- temp = b_A[jy] * -1.0F;
- ix = jp1j;
- loop_ub = (mmj + jj) + 18;
- for (k = 10 + jj; k + 1 <= loop_ub; k++) {
- b_A[k] += b_A[ix] * temp;
+ c = A_sizes[1] - j;
+ jA = jj + ldap1;
+ iy = jj + A_sizes[1];
+ for (jrow = 1; jrow <= c; jrow++) {
+ if ((real_T)b_A_data[iy] != 0.0) {
+ temp = b_A_data[iy] * -1.0F;
+ ix = jj;
+ b_loop_ub = (mmj + jA) - 1;
+ for (k = jA; k + 1 <= b_loop_ub; k++) {
+ b_A_data[k] += b_A_data[ix + 1] * temp;
ix++;
}
}
- jy += 9;
- jj += 9;
+ iy += A_sizes[1];
+ jA += A_sizes[1];
}
}
- for (jy = 0; jy < 12; jy++) {
- for (iy = 0; iy < 9; iy++) {
- Y[iy + 9 * jy] = A[jy + 12 * iy];
+ for (jA = 0; jA + 1 <= A_sizes[1]; jA++) {
+ if (ipiv_data[jA] != jA + 1) {
+ for (j = 0; j < 9; j++) {
+ temp = B_data[jA + B_sizes[0] * j];
+ B_data[jA + B_sizes[0] * j] = B_data[(ipiv_data[jA] + B_sizes[0] * j) -
+ 1];
+ B_data[(ipiv_data[jA] + B_sizes[0] * j) - 1] = temp;
+ }
}
}
- for (iy = 0; iy < 9; iy++) {
- if (ipiv[iy] != iy + 1) {
- for (j = 0; j < 12; j++) {
- temp = Y[iy + 9 * j];
- Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1];
- Y[(ipiv[iy] + 9 * j) - 1] = temp;
+ if (B_sizes[0] == 0) {
+ } else {
+ for (j = 0; j < 9; j++) {
+ c = A_sizes[1] * j;
+ for (k = 0; k + 1 <= A_sizes[1]; k++) {
+ iy = A_sizes[1] * k;
+ if ((real_T)B_data[k + c] != 0.0) {
+ for (jA = k + 1; jA + 1 <= A_sizes[1]; jA++) {
+ B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy];
+ }
+ }
}
}
}
- for (j = 0; j < 12; j++) {
- c = 9 * j;
- for (k = 0; k < 9; k++) {
- jy = 9 * k;
- if ((real_T)Y[k + c] != 0.0) {
- for (iy = k + 2; iy < 10; iy++) {
- Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
+ if (B_sizes[0] == 0) {
+ } else {
+ for (j = 0; j < 9; j++) {
+ c = A_sizes[1] * j;
+ for (k = A_sizes[1] - 1; k + 1 > 0; k--) {
+ iy = A_sizes[1] * k;
+ if ((real_T)B_data[k + c] != 0.0) {
+ B_data[k + c] /= b_A_data[k + iy];
+ for (jA = 0; jA + 1 <= k; jA++) {
+ B_data[jA + c] -= B_data[k + c] * b_A_data[jA + iy];
+ }
}
}
}
}
+}
+
+/*
+ *
+ */
+static real32_T eml_matlab_zlarfg(int32_T n, real32_T *alpha1, real32_T x_data
+ [81], int32_T x_sizes[2], int32_T ix0)
+{
+ real32_T tau;
+ int32_T nm1;
+ real32_T xnorm;
+ real32_T a;
+ int32_T knt;
+ int32_T loop_ub;
+ int32_T k;
+ tau = 0.0F;
+ if (n <= 0) {
+ } else {
+ nm1 = n - 2;
+ xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0);
+ if ((real_T)xnorm != 0.0) {
+ a = (real32_T)fabs(*alpha1);
+ xnorm = (real32_T)fabs(xnorm);
+ if (a < xnorm) {
+ a /= xnorm;
+ xnorm *= (real32_T)sqrt(a * a + 1.0F);
+ } else if (a > xnorm) {
+ xnorm /= a;
+ xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a;
+ } else if (rtIsNaNF(xnorm)) {
+ } else {
+ xnorm = a * 1.41421354F;
+ }
+
+ if ((real_T)*alpha1 >= 0.0) {
+ xnorm = -xnorm;
+ }
+
+ if ((real32_T)fabs(xnorm) < 9.86076132E-32F) {
+ knt = 0;
+ do {
+ knt++;
+ loop_ub = ix0 + nm1;
+ for (k = ix0; k <= loop_ub; k++) {
+ x_data[k - 1] *= 1.01412048E+31F;
+ }
+
+ xnorm *= 1.01412048E+31F;
+ *alpha1 *= 1.01412048E+31F;
+ } while (!((real32_T)fabs(xnorm) >= 9.86076132E-32F));
+
+ xnorm = eml_xnrm2(nm1 + 1, x_data, x_sizes, ix0);
+ a = (real32_T)fabs(*alpha1);
+ xnorm = (real32_T)fabs(xnorm);
+ if (a < xnorm) {
+ a /= xnorm;
+ xnorm *= (real32_T)sqrt(a * a + 1.0F);
+ } else if (a > xnorm) {
+ xnorm /= a;
+ xnorm = (real32_T)sqrt(xnorm * xnorm + 1.0F) * a;
+ } else if (rtIsNaNF(xnorm)) {
+ } else {
+ xnorm = a * 1.41421354F;
+ }
+
+ if ((real_T)*alpha1 >= 0.0) {
+ xnorm = -xnorm;
+ }
+
+ tau = (xnorm - *alpha1) / xnorm;
+ *alpha1 = 1.0F / (*alpha1 - xnorm);
+ loop_ub = ix0 + nm1;
+ for (k = ix0; k <= loop_ub; k++) {
+ x_data[k - 1] *= *alpha1;
+ }
+
+ for (k = 1; k <= knt; k++) {
+ xnorm *= 9.86076132E-32F;
+ }
- for (j = 0; j < 12; j++) {
- c = 9 * j;
- for (k = 8; k > -1; k += -1) {
- jy = 9 * k;
- if ((real_T)Y[k + c] != 0.0) {
- Y[k + c] /= b_A[k + jy];
- for (iy = 0; iy + 1 <= k; iy++) {
- Y[iy + c] -= Y[k + c] * b_A[iy + jy];
+ *alpha1 = xnorm;
+ } else {
+ tau = (xnorm - *alpha1) / xnorm;
+ *alpha1 = 1.0F / (*alpha1 - xnorm);
+ loop_ub = ix0 + nm1;
+ for (k = ix0; k <= loop_ub; k++) {
+ x_data[k - 1] *= *alpha1;
}
+
+ *alpha1 = xnorm;
}
}
}
- for (jy = 0; jy < 9; jy++) {
- for (iy = 0; iy < 12; iy++) {
- y[iy + 12 * jy] = Y[jy + 9 * iy];
+ return tau;
+}
+
+/*
+ *
+ */
+static void eml_qrsolve(const real32_T A_data[81], const int32_T A_sizes[2],
+ real32_T B_data[81], int32_T B_sizes[2], real32_T Y_data[81], int32_T Y_sizes
+ [2])
+{
+ real_T rankR;
+ real_T u1;
+ int32_T mn;
+ int32_T b_A_sizes[2];
+ int32_T loop_ub;
+ int32_T k;
+ real32_T b_A_data[81];
+ int32_T pvt;
+ int32_T b_mn;
+ int32_T tmp_data[9];
+ int32_T jpvt_data[9];
+ int8_T unnamed_idx_0;
+ real32_T work_data[9];
+ int32_T nmi;
+ real32_T vn1_data[9];
+ real32_T vn2_data[9];
+ int32_T i;
+ int32_T i_i;
+ int32_T mmi;
+ int32_T ix;
+ real32_T smax;
+ real32_T s;
+ int32_T iy;
+ real32_T atmp;
+ real32_T tau_data[9];
+ int32_T i_ip1;
+ int32_T lastv;
+ int32_T lastc;
+ boolean_T exitg2;
+ int32_T exitg1;
+ boolean_T firstNonZero;
+ real32_T t;
+ rankR = (real_T)A_sizes[0];
+ u1 = (real_T)A_sizes[1];
+ rankR = rankR <= u1 ? rankR : u1;
+ mn = (int32_T)rankR;
+ b_A_sizes[0] = A_sizes[0];
+ b_A_sizes[1] = A_sizes[1];
+ loop_ub = A_sizes[0] * A_sizes[1] - 1;
+ for (k = 0; k <= loop_ub; k++) {
+ b_A_data[k] = A_data[k];
+ }
+
+ k = A_sizes[0];
+ pvt = A_sizes[1];
+ b_mn = k <= pvt ? k : pvt;
+ if (A_sizes[1] < 1) {
+ } else {
+ loop_ub = A_sizes[1] - 1;
+ for (k = 0; k <= loop_ub; k++) {
+ tmp_data[k] = 1 + k;
+ }
+
+ loop_ub = A_sizes[1] - 1;
+ for (k = 0; k <= loop_ub; k++) {
+ jpvt_data[k] = tmp_data[k];
+ }
+ }
+
+ if ((A_sizes[0] == 0) || (A_sizes[1] == 0)) {
+ } else {
+ unnamed_idx_0 = (int8_T)A_sizes[1];
+ loop_ub = unnamed_idx_0 - 1;
+ for (k = 0; k <= loop_ub; k++) {
+ work_data[k] = 0.0F;
+ }
+
+ k = 1;
+ for (nmi = 0; nmi + 1 <= A_sizes[1]; nmi++) {
+ vn1_data[nmi] = eml_xnrm2(A_sizes[0], A_data, A_sizes, k);
+ vn2_data[nmi] = vn1_data[nmi];
+ k += A_sizes[0];
+ }
+
+ for (i = 0; i + 1 <= b_mn; i++) {
+ i_i = i + i * A_sizes[0];
+ nmi = A_sizes[1] - i;
+ mmi = (A_sizes[0] - i) - 1;
+ if (nmi < 1) {
+ pvt = -1;
+ } else {
+ pvt = 0;
+ if (nmi > 1) {
+ ix = i;
+ smax = (real32_T)fabs(vn1_data[i]);
+ for (k = 1; k + 1 <= nmi; k++) {
+ ix++;
+ s = (real32_T)fabs(vn1_data[ix]);
+ if (s > smax) {
+ pvt = k;
+ smax = s;
+ }
+ }
+ }
+ }
+
+ pvt += i;
+ if (pvt + 1 != i + 1) {
+ ix = A_sizes[0] * pvt;
+ iy = A_sizes[0] * i;
+ for (k = 1; k <= A_sizes[0]; k++) {
+ s = b_A_data[ix];
+ b_A_data[ix] = b_A_data[iy];
+ b_A_data[iy] = s;
+ ix++;
+ iy++;
+ }
+
+ k = jpvt_data[pvt];
+ jpvt_data[pvt] = jpvt_data[i];
+ jpvt_data[i] = k;
+ vn1_data[pvt] = vn1_data[i];
+ vn2_data[pvt] = vn2_data[i];
+ }
+
+ if (i + 1 < A_sizes[0]) {
+ atmp = b_A_data[i_i];
+ smax = eml_matlab_zlarfg(mmi + 1, &atmp, b_A_data, b_A_sizes, i_i + 2);
+ tau_data[i] = smax;
+ } else {
+ atmp = b_A_data[i_i];
+ smax = b_A_data[i_i];
+ s = b_eml_matlab_zlarfg(&atmp, &smax);
+ b_A_data[i_i] = smax;
+ tau_data[i] = s;
+ }
+
+ b_A_data[i_i] = atmp;
+ if (i + 1 < A_sizes[1]) {
+ atmp = b_A_data[i_i];
+ b_A_data[i_i] = 1.0F;
+ i_ip1 = (i + (i + 1) * A_sizes[0]) + 1;
+ if ((real_T)tau_data[i] != 0.0) {
+ lastv = mmi;
+ pvt = i_i + mmi;
+ while ((lastv + 1 > 0) && ((real_T)b_A_data[pvt] == 0.0)) {
+ lastv--;
+ pvt--;
+ }
+
+ lastc = nmi - 1;
+ exitg2 = 0U;
+ while ((exitg2 == 0U) && (lastc > 0)) {
+ k = i_ip1 + (lastc - 1) * A_sizes[0];
+ pvt = k + lastv;
+ do {
+ exitg1 = 0U;
+ if (k <= pvt) {
+ if ((real_T)b_A_data[k - 1] != 0.0) {
+ exitg1 = 1U;
+ } else {
+ k++;
+ }
+ } else {
+ lastc--;
+ exitg1 = 2U;
+ }
+ } while (exitg1 == 0U);
+
+ if (exitg1 == 1U) {
+ exitg2 = 1U;
+ }
+ }
+ } else {
+ lastv = -1;
+ lastc = 0;
+ }
+
+ if (lastv + 1 > 0) {
+ if (lastc == 0) {
+ } else {
+ for (iy = 1; iy <= lastc; iy++) {
+ work_data[iy - 1] = 0.0F;
+ }
+
+ iy = 0;
+ loop_ub = i_ip1 + A_sizes[0] * (lastc - 1);
+ pvt = i_ip1;
+ while ((A_sizes[0] > 0) && (pvt <= loop_ub)) {
+ ix = i_i;
+ smax = 0.0F;
+ k = pvt + lastv;
+ for (nmi = pvt; nmi <= k; nmi++) {
+ smax += b_A_data[nmi - 1] * b_A_data[ix];
+ ix++;
+ }
+
+ work_data[iy] += smax;
+ iy++;
+ pvt += A_sizes[0];
+ }
+ }
+
+ smax = -tau_data[i];
+ if ((real_T)smax == 0.0) {
+ } else {
+ pvt = 0;
+ for (nmi = 1; nmi <= lastc; nmi++) {
+ if ((real_T)work_data[pvt] != 0.0) {
+ s = work_data[pvt] * smax;
+ ix = i_i;
+ loop_ub = lastv + i_ip1;
+ for (k = i_ip1 - 1; k + 1 <= loop_ub; k++) {
+ b_A_data[k] += b_A_data[ix] * s;
+ ix++;
+ }
+ }
+
+ pvt++;
+ i_ip1 += A_sizes[0];
+ }
+ }
+ }
+
+ b_A_data[i_i] = atmp;
+ }
+
+ for (nmi = i + 1; nmi + 1 <= A_sizes[1]; nmi++) {
+ if ((real_T)vn1_data[nmi] != 0.0) {
+ smax = (real32_T)fabs(b_A_data[i + b_A_sizes[0] * nmi]) / vn1_data[nmi];
+ smax = 1.0F - smax * smax;
+ if ((real_T)smax < 0.0) {
+ smax = 0.0F;
+ }
+
+ s = vn1_data[nmi] / vn2_data[nmi];
+ if (smax * (s * s) <= 0.000345266977F) {
+ if (i + 1 < A_sizes[0]) {
+ k = (i + A_sizes[0] * nmi) + 1;
+ smax = 0.0F;
+ if (mmi < 1) {
+ } else if (mmi == 1) {
+ smax = (real32_T)fabs(b_A_data[k]);
+ } else {
+ s = 0.0F;
+ firstNonZero = TRUE;
+ pvt = k + mmi;
+ while (k + 1 <= pvt) {
+ if (b_A_data[k] != 0.0F) {
+ atmp = (real32_T)fabs(b_A_data[k]);
+ if (firstNonZero) {
+ s = atmp;
+ smax = 1.0F;
+ firstNonZero = FALSE;
+ } else if (s < atmp) {
+ t = s / atmp;
+ smax = 1.0F + smax * t * t;
+ s = atmp;
+ } else {
+ t = atmp / s;
+ smax += t * t;
+ }
+ }
+
+ k++;
+ }
+
+ smax = s * (real32_T)sqrt(smax);
+ }
+
+ vn1_data[nmi] = smax;
+ vn2_data[nmi] = vn1_data[nmi];
+ } else {
+ vn1_data[nmi] = 0.0F;
+ vn2_data[nmi] = 0.0F;
+ }
+ } else {
+ vn1_data[nmi] *= (real32_T)sqrt(smax);
+ }
+ }
+ }
+ }
+ }
+
+ rankR = (real_T)A_sizes[0];
+ u1 = (real_T)A_sizes[1];
+ rankR = rankR >= u1 ? rankR : u1;
+ smax = (real32_T)rankR * (real32_T)fabs(b_A_data[0]) * 1.1920929E-7F;
+ rankR = 0.0;
+ k = 0;
+ while ((k + 1 <= mn) && (!((real32_T)fabs(b_A_data[k + b_A_sizes[0] * k]) <=
+ smax))) {
+ rankR++;
+ k++;
+ }
+
+ unnamed_idx_0 = (int8_T)A_sizes[1];
+ Y_sizes[0] = (int32_T)unnamed_idx_0;
+ Y_sizes[1] = 9;
+ loop_ub = unnamed_idx_0 * 9 - 1;
+ for (k = 0; k <= loop_ub; k++) {
+ Y_data[k] = 0.0F;
+ }
+
+ for (nmi = 0; nmi + 1 <= mn; nmi++) {
+ if ((real_T)tau_data[nmi] != 0.0) {
+ for (k = 0; k < 9; k++) {
+ smax = B_data[nmi + B_sizes[0] * k];
+ for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) {
+ smax += b_A_data[i + b_A_sizes[0] * nmi] * B_data[i + B_sizes[0] * k];
+ }
+
+ smax *= tau_data[nmi];
+ if ((real_T)smax != 0.0) {
+ B_data[nmi + B_sizes[0] * k] -= smax;
+ for (i = nmi + 1; i + 1 <= A_sizes[0]; i++) {
+ B_data[i + B_sizes[0] * k] -= b_A_data[i + b_A_sizes[0] * nmi] *
+ smax;
+ }
+ }
+ }
+ }
+ }
+
+ for (k = 0; k < 9; k++) {
+ for (i = 0; (real_T)(i + 1) <= rankR; i++) {
+ Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] = B_data[i + B_sizes[0] * k];
+ }
+
+ for (nmi = (int32_T)rankR - 1; nmi + 1 >= 1; nmi--) {
+ Y_data[(jpvt_data[nmi] + Y_sizes[0] * k) - 1] /= b_A_data[nmi + b_A_sizes
+ [0] * nmi];
+ for (i = 0; i + 1 <= nmi; i++) {
+ Y_data[(jpvt_data[i] + Y_sizes[0] * k) - 1] -= Y_data[(jpvt_data[nmi] +
+ Y_sizes[0] * k) - 1] * b_A_data[i + b_A_sizes[0] * nmi];
+ }
+ }
+ }
+}
+
+/*
+ *
+ */
+static real32_T eml_xnrm2(int32_T n, const real32_T x_data[81], const int32_T
+ x_sizes[2], int32_T ix0)
+{
+ real32_T y;
+ real32_T scale;
+ boolean_T firstNonZero;
+ int32_T kend;
+ int32_T k;
+ real32_T absxk;
+ real32_T t;
+ y = 0.0F;
+ if (n < 1) {
+ } else if (n == 1) {
+ y = (real32_T)fabs(x_data[ix0 - 1]);
+ } else {
+ scale = 0.0F;
+ firstNonZero = TRUE;
+ kend = (ix0 + n) - 1;
+ for (k = ix0 - 1; k + 1 <= kend; k++) {
+ if (x_data[k] != 0.0F) {
+ absxk = (real32_T)fabs(x_data[k]);
+ if (firstNonZero) {
+ scale = absxk;
+ y = 1.0F;
+ firstNonZero = FALSE;
+ } else if (scale < absxk) {
+ t = scale / absxk;
+ y = 1.0F + y * t * t;
+ scale = absxk;
+ } else {
+ t = absxk / scale;
+ y += t * t;
+ }
+ }
+ }
+
+ y = scale * (real32_T)sqrt(y);
+ }
+
+ return y;
+}
+
+/*
+ *
+ */
+void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const
+ real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81],
+ int32_T y_sizes[2])
+{
+ int32_T b_A_sizes[2];
+ int32_T loop_ub;
+ int32_T i3;
+ int32_T b_loop_ub;
+ int32_T i4;
+ real32_T b_A_data[81];
+ int32_T b_B_sizes[2];
+ real32_T b_B_data[81];
+ int8_T unnamed_idx_0;
+ int32_T c_B_sizes[2];
+ real32_T c_B_data[81];
+ b_A_sizes[0] = B_sizes[1];
+ b_A_sizes[1] = B_sizes[0];
+ loop_ub = B_sizes[0] - 1;
+ for (i3 = 0; i3 <= loop_ub; i3++) {
+ b_loop_ub = B_sizes[1] - 1;
+ for (i4 = 0; i4 <= b_loop_ub; i4++) {
+ b_A_data[i4 + b_A_sizes[0] * i3] = B_data[i3 + B_sizes[0] * i4];
+ }
+ }
+
+ b_B_sizes[0] = A_sizes[1];
+ b_B_sizes[1] = 9;
+ for (i3 = 0; i3 < 9; i3++) {
+ loop_ub = A_sizes[1] - 1;
+ for (i4 = 0; i4 <= loop_ub; i4++) {
+ b_B_data[i4 + b_B_sizes[0] * i3] = A_data[i3 + A_sizes[0] * i4];
+ }
+ }
+
+ if ((b_A_sizes[0] == 0) || (b_A_sizes[1] == 0) || (b_B_sizes[0] == 0)) {
+ unnamed_idx_0 = (int8_T)b_A_sizes[1];
+ b_B_sizes[0] = (int32_T)unnamed_idx_0;
+ b_B_sizes[1] = 9;
+ loop_ub = unnamed_idx_0 * 9 - 1;
+ for (i3 = 0; i3 <= loop_ub; i3++) {
+ b_B_data[i3] = 0.0F;
+ }
+ } else if (b_A_sizes[0] == b_A_sizes[1]) {
+ eml_lusolve(b_A_data, b_A_sizes, b_B_data, b_B_sizes);
+ } else {
+ c_B_sizes[0] = b_B_sizes[0];
+ c_B_sizes[1] = 9;
+ loop_ub = b_B_sizes[0] * 9 - 1;
+ for (i3 = 0; i3 <= loop_ub; i3++) {
+ c_B_data[i3] = b_B_data[i3];
+ }
+
+ eml_qrsolve(b_A_data, b_A_sizes, c_B_data, c_B_sizes, b_B_data, b_B_sizes);
+ }
+
+ y_sizes[0] = 9;
+ y_sizes[1] = b_B_sizes[0];
+ loop_ub = b_B_sizes[0] - 1;
+ for (i3 = 0; i3 <= loop_ub; i3++) {
+ for (i4 = 0; i4 < 9; i4++) {
+ y_data[i4 + 9 * i3] = b_B_data[i3 + b_B_sizes[0] * i4];
}
}
}
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h index e81bfffc0..a58faaa26 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -3,7 +3,7 @@ *
* Code generation for function 'mrdivide'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:23 2012
*
*/
@@ -14,6 +14,8 @@ #include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
@@ -27,6 +29,6 @@ /* Variable Definitions */
/* Function Declarations */
-extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
+extern void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], int32_T y_sizes[2]);
#endif
/* End of code generation (mrdivide.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index 1fbde052b..8d8e4e110 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -3,7 +3,7 @@ *
* Code generation for function 'norm'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:23 2012
*
*/
@@ -40,7 +40,7 @@ real32_T norm(const real32_T x[3]) firstNonZero = TRUE;
for (k = 0; k < 3; k++) {
if (x[k] != 0.0F) {
- absxk = fabsf(x[k]);
+ absxk = (real32_T)fabs(x[k]);
if (firstNonZero) {
scale = absxk;
y = 1.0F;
@@ -56,7 +56,7 @@ real32_T norm(const real32_T x[3]) }
}
- return scale * sqrtf(y);
+ return scale * (real32_T)sqrt(y);
}
/* End of code generation (norm.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h index b0c7fe430..479503ae3 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -3,7 +3,7 @@ *
* Code generation for function 'norm'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:23 2012
*
*/
@@ -14,6 +14,8 @@ #include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index 3cef17684..42b3af501 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:24 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index ab2d5a70d..20d8c7f05 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:24 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index 17a093461..d29aea34b 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:24 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index 2c254bbbf..a14b6170b 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:24 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h new file mode 100755 index 000000000..4cad63ada --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -0,0 +1,24 @@ +/*
+ * rt_defines.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Mon Sep 17 20:13:24 2012
+ *
+ */
+
+#ifndef __RT_DEFINES_H__
+#define __RT_DEFINES_H__
+
+#include <stdlib.h>
+
+#define RT_PI 3.14159265358979323846
+#define RT_PIF 3.1415927F
+#define RT_LN_10 2.30258509299404568402
+#define RT_LN_10F 2.3025851F
+#define RT_LOG10E 0.43429448190325182765
+#define RT_LOG10EF 0.43429449F
+#define RT_E 2.7182818284590452354
+#define RT_EF 2.7182817F
+#endif
+/* End of code generation (rt_defines.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index 005c4f28d..bd04f52e6 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:24 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index 6481f011f..67b3ba205 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:24 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw b/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw deleted file mode 100755 index 1fb585abd..000000000 --- a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw +++ /dev/null @@ -1 +0,0 @@ -Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a.
diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index fd629ebcd..6feb2e1a9 100755 --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Wed Jul 11 08:38:35 2012
+ * C source code generated on: Mon Sep 17 20:13:24 2012
*
*/
|