dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Updated EKF filter, fixed uploader (reverted to master)

This commit is contained in:
Lorenz Meier 2012-10-02 09:23:47 +02:00
parent d3e7b5e0bf
commit 9de6a81543
31 changed files with 1112 additions and 1374 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
Tools/px_uploader.py
View File

@ -263,8 +263,8 @@ class uploader(object):
print("program...")
self.__program(fw)
#print("verify...")
#self.__verify(fw)
print("verify...")
self.__verify(fw)
print("done, rebooting.")
self.__reboot()
@ -290,7 +290,18 @@ while True:
# create an uploader attached to the port
try:
up = uploader("\\\\.\\COM2", args.baud)
if "linux" in _platform:
# Linux, don't open Mac OS and Win ports
if not "COM" in port and not "tty.usb" in port:
up = uploader(port, args.baud)
elif "darwin" in _platform:
# OS X, don't open Windows and Linux ports
if not "COM" in port and not "ACM" in port:
up = uploader(port, args.baud)
elif "win" in _platform:
# Windows, don't open POSIX ports
if not "/" in port:
up = uploader(port, args.baud)
except:
# open failed, rate-limit our attempts
time.sleep(0.05)

4
apps/attitude_estimator_ekf/Makefile
View File

@ -45,9 +45,9 @@ CSRCS = attitude_estimator_ekf_main.c \
codegen/rtGetInf.c \
codegen/rtGetNaN.c \
codegen/norm.c \
codegen/find.c \
codegen/diag.c \
codegen/cross.c
codegen/cross.c \
codegen/power.c
# XXX this is *horribly* broken
INCLUDES += $(TOPDIR)/../mavlink/include/mavlink

135
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
View File

@ -75,38 +75,45 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
static float dt = 1;
static float dt = 1.0f;
/* 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_k[9] = {0,0,0,0,0,-9.81,1,1,-1}; /**< */
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,
static float z_k[9]; /**< Measurement vector */
static float x_aposteriori_k[12]; /**< */
static float x_aposteriori[12];
static float P_aposteriori_k[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.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
};
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,
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.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
// static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 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
}; /**< init: identity matrix */
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */
@ -229,6 +236,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
/* process noise covariance */
float q[12];
/* measurement noise covariance */
float r[9];
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f};
/* Main loop*/
while (!thread_should_exit) {
@ -278,10 +293,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
overloadcounter++;
}
int8_t update_vect[3] = {1, 1, 1};
float euler[3];
uint8_t update_vect[3] = {1, 1, 1};
int32_t z_k_sizes = 9;
float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
static bool const_initialized = false;
@ -289,38 +303,41 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/)
{
dt = 0.005f;
q[0] = 1e1;
q[1] = 1e1;
q[2] = 1e1;
q[3] = 1e-6;
q[4] = 1e-6;
q[5] = 1e-6;
q[6] = 1e-1;
q[7] = 1e-1;
q[8] = 1e-1;
q[9] = 1e-1;
q[10] = 1e-1;
q[11] = 1e-1;
q[0] = 1e1f;
q[1] = 1e1f;
q[2] = 1e1f;
q[3] = 1e-6f;
q[4] = 1e-6f;
q[5] = 1e-6f;
q[6] = 1e-1f;
q[7] = 1e-1f;
q[8] = 1e-1f;
q[9] = 1e-1f;
q[10] = 1e-1f;
q[11] = 1e-1f;
r[0]= 1e-2;
r[1]= 1e-2;
r[2]= 1e-2;
r[3]= 1e-1;
r[4]= 1e-1;
r[5]= 1e-1;
r[6]= 1e-1;
r[7]= 1e-1;
r[8]= 1e-1;
r[0]= 1e-2f;
r[1]= 1e-2f;
r[2]= 1e-2f;
r[3]= 1e-1f;
r[4]= 1e-1f;
r[5]= 1e-1f;
r[6]= 1e-1f;
r[7]= 1e-1f;
r[8]= 1e-1f;
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];
x_aposteriori_k[3] = z_k[3];
x_aposteriori_k[4] = z_k[4];
x_aposteriori_k[5] = z_k[5];
x_aposteriori_k[6] = z_k[6];
x_aposteriori_k[7] = z_k[7];
x_aposteriori_k[8] = z_k[8];
x_aposteriori_k[3] = 0.0f;
x_aposteriori_k[4] = 0.0f;
x_aposteriori_k[5] = 0.0f;
x_aposteriori_k[6] = z_k[3];
x_aposteriori_k[7] = z_k[4];
x_aposteriori_k[8] = z_k[5];
x_aposteriori_k[9] = z_k[6];
x_aposteriori_k[10] = z_k[7];
x_aposteriori_k[11] = z_k[8];
const_initialized = true;
}
@ -331,15 +348,17 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
}
uint64_t timing_start = hrt_absolute_time();
attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
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);
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, q, r,
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 % 2 == 0) {
if (printcounter % 200 == 0) {
// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
// x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
// x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
@ -348,8 +367,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
// }
// 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", (double)euler[0], (double)euler[1], (double)euler[2]);
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\n", (double)euler[0], (double)euler[1], (double)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));

1140
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
View File

File diff suppressed because it is too large Load Diff

4
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/
@ -29,6 +29,6 @@
/* Variable Definitions */
/* Function Declarations */
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]);
extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
#endif
/* End of code generation (attitudeKalmanfilter.h) */

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:42 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/cross.c
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'cross'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/cross.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'cross'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

40
apps/attitude_estimator_ekf/codegen/diag.c
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'diag'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/
@ -27,7 +27,19 @@
/*
*
*/
void diag(const real32_T v[3], real32_T d[9])
void b_diag(const real32_T v[9], real32_T d[81])
{
int32_T j;
memset((void *)&d[0], 0, 81U * sizeof(real32_T));
for (j = 0; j < 9; j++) {
d[j + 9 * j] = v[j];
}
}
/*
*
*/
void c_diag(const real32_T v[3], real32_T d[9])
{
int32_T j;
for (j = 0; j < 9; j++) {
@ -39,4 +51,28 @@ void diag(const real32_T v[3], real32_T d[9])
}
}
/*
*
*/
void d_diag(const real32_T v[6], real32_T d[36])
{
int32_T j;
memset((void *)&d[0], 0, 36U * sizeof(real32_T));
for (j = 0; j < 6; j++) {
d[j + 6 * j] = v[j];
}
}
/*
*
*/
void diag(const real32_T v[12], real32_T d[144])
{
int32_T j;
memset((void *)&d[0], 0, 144U * sizeof(real32_T));
for (j = 0; j < 12; j++) {
d[j + 12 * j] = v[j];
}
}
/* End of code generation (diag.c) */

7
apps/attitude_estimator_ekf/codegen/diag.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'diag'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/
@ -29,6 +29,9 @@
/* Variable Definitions */
/* Function Declarations */
extern void diag(const real32_T v[3], real32_T d[9]);
extern void b_diag(const real32_T v[9], real32_T d[81]);
extern void c_diag(const real32_T v[3], real32_T d[9]);
extern void d_diag(const real32_T v[6], real32_T d[36]);
extern void diag(const real32_T v[12], real32_T d[144]);
#endif
/* End of code generation (diag.h) */

10
apps/attitude_estimator_ekf/codegen/eye.c
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/
@ -27,12 +27,12 @@
/*
*
*/
void b_eye(real_T I[81])
void b_eye(real_T I[144])
{
int32_T i;
memset((void *)&I[0], 0, 81U * sizeof(real_T));
for (i = 0; i < 9; i++) {
I[i + 9 * i] = 1.0;
memset((void *)&I[0], 0, 144U * sizeof(real_T));
for (i = 0; i < 12; i++) {
I[i + 12 * i] = 1.0;
}
}

4
apps/attitude_estimator_ekf/codegen/eye.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'eye'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/
@ -29,7 +29,7 @@
/* Variable Definitions */
/* Function Declarations */
extern void b_eye(real_T I[81]);
extern void b_eye(real_T I[144]);
extern void eye(real_T I[9]);
#endif
/* End of code generation (eye.h) */

77
apps/attitude_estimator_ekf/codegen/find.c
View File

@ -1,77 +0,0 @@
/*
* find.c
*
* Code generation for function 'find'
*
* C source code generated on: Fri Sep 21 13:56:43 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) */

898
apps/attitude_estimator_ekf/codegen/mrdivide.c
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/
@ -21,719 +21,345 @@
/* 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 */
/*
*
*/
static real32_T b_eml_matlab_zlarfg(real32_T *alpha1, const real32_T *x)
void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
{
return 0.0F;
int32_T rtemp;
int32_T k;
real32_T b_A[9];
real32_T b_B[36];
int32_T r1;
int32_T r2;
int32_T r3;
real32_T maxval;
real32_T a21;
real32_T Y[36];
for (rtemp = 0; rtemp < 3; rtemp++) {
for (k = 0; k < 3; k++) {
b_A[k + 3 * rtemp] = B[rtemp + 3 * k];
}
}
for (rtemp = 0; rtemp < 12; rtemp++) {
for (k = 0; k < 3; k++) {
b_B[k + 3 * rtemp] = A[rtemp + 12 * k];
}
}
r1 = 0;
r2 = 1;
r3 = 2;
maxval = (real32_T)fabsf(b_A[0]);
a21 = (real32_T)fabsf(b_A[1]);
if (a21 > maxval) {
maxval = a21;
r1 = 1;
r2 = 0;
}
if ((real32_T)fabsf(b_A[2]) > maxval) {
r1 = 2;
r2 = 1;
r3 = 0;
}
b_A[r2] /= b_A[r1];
b_A[r3] /= b_A[r1];
b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
if ((real32_T)fabsf(b_A[3 + r3]) > (real32_T)fabsf(b_A[3 + r2])) {
rtemp = r2;
r2 = r3;
r3 = rtemp;
}
b_A[3 + r3] /= b_A[3 + r2];
b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
for (k = 0; k < 12; k++) {
Y[3 * k] = b_B[r1 + 3 * k];
Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];
Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3
+ r3];
Y[2 + 3 * k] /= b_A[6 + r3];
Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];
Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];
Y[1 + 3 * k] /= b_A[3 + r2];
Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];
Y[3 * k] /= b_A[r1];
}
for (rtemp = 0; rtemp < 3; rtemp++) {
for (k = 0; k < 12; k++) {
y[k + 12 * rtemp] = Y[rtemp + 3 * k];
}
}
}
/*
*
*/
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])
void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
{
int32_T loop_ub;
int32_T jy;
int32_T iy;
real32_T b_A_data[81];
int32_T jA;
int32_T tmp_data[9];
int32_T ipiv_data[9];
int32_T ldap1;
real32_T b_A[36];
int8_T ipiv[6];
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 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];
int32_T loop_ub;
real32_T Y[72];
for (jy = 0; jy < 6; jy++) {
for (iy = 0; iy < 6; iy++) {
b_A[iy + 6 * jy] = B[jy + 6 * 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];
}
}
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) {
for (j = 0; j < 5; j++) {
mmj = -j;
jj = j * 7;
jp1j = jj + 1;
c = mmj + 6;
jy = 0;
ix = jj;
temp = (real32_T)fabs(b_A_data[jj]);
for (k = 1; k + 1 <= mmj; k++) {
temp = (real32_T)fabsf(b_A[jj]);
for (k = 2; k <= c; k++) {
ix++;
s = (real32_T)fabs(b_A_data[ix]);
s = (real32_T)fabsf(b_A[ix]);
if (s > temp) {
jA = k;
jy = k - 1;
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 < 6; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
b_A[iy] = temp;
ix += 6;
iy += 6;
}
}
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) + 5;
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 = 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;
c = 5 - j;
jy = jj + 6;
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) + 12;
for (k = 7 + jj; k + 1 <= loop_ub; k++) {
b_A[k] += b_A[ix] * temp;
ix++;
}
}
iy += A_sizes[1];
jA += A_sizes[1];
jy += 6;
jj += 6;
}
}
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 (jy = 0; jy < 12; jy++) {
for (iy = 0; iy < 6; iy++) {
Y[iy + 6 * jy] = A[jy + 12 * iy];
}
}
for (iy = 0; iy < 6; iy++) {
if (ipiv[iy] != iy + 1) {
for (j = 0; j < 12; j++) {
temp = Y[iy + 6 * j];
Y[iy + 6 * j] = Y[(ipiv[iy] + 6 * j) - 1];
Y[(ipiv[iy] + 6 * 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 = 6 * j;
for (k = 0; k < 6; k++) {
jy = 6 * k;
if ((real_T)Y[k + c] != 0.0) {
for (iy = k + 2; iy < 7; 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];
for (j = 0; j < 12; j++) {
c = 6 * j;
for (k = 5; k > -1; k += -1) {
jy = 6 * 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];
}
}
}
}
for (jy = 0; jy < 6; jy++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * jy] = Y[jy + 6 * 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)
void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
{
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;
}
*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;
}
}
}
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 jy;
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;
real32_T b_A[81];
int8_T ipiv[9];
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 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];
real32_T s;
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];
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];
}
ipiv[jy] = (int8_T)(1 + jy);
}
for (j = 0; j < 8; j++) {
mmj = -j;
jj = j * 10;
jp1j = jj + 1;
c = mmj + 9;
jy = 0;
ix = jj;
temp = (real32_T)fabsf(b_A[jj]);
for (k = 2; k <= c; k++) {
ix++;
s = (real32_T)fabsf(b_A[ix]);
if (s > temp) {
jy = k - 1;
temp = s;
}
}
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 ((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 ((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;
loop_ub = (jp1j + mmj) + 8;
for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
b_A[iy] /= b_A[jj];
}
} 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);
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;
ix++;
}
}
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];
jy += 9;
jj += 9;
}
}
for (jy = 0; jy < 12; jy++) {
for (iy = 0; iy < 9; iy++) {
Y[iy + 9 * jy] = A[jy + 12 * iy];
}
}
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;
}
}
}
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];
}
}
}
}
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];
}
}
}
}
for (jy = 0; jy < 9; jy++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * jy] = Y[jy + 9 * iy];
}
}
}

6
apps/attitude_estimator_ekf/codegen/mrdivide.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/
@ -29,6 +29,8 @@
/* Variable Definitions */
/* Function Declarations */
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]);
extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
#endif
/* End of code generation (mrdivide.h) */

2
apps/attitude_estimator_ekf/codegen/norm.c
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/norm.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'norm'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

84
apps/attitude_estimator_ekf/codegen/power.c Executable file
View File

@ -0,0 +1,84 @@
/*
* power.c
*
* Code generation for function 'power'
*
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "power.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
static real32_T rt_powf_snf(real32_T u0, real32_T u1);
/* Function Definitions */
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
{
real32_T y;
real32_T f0;
real32_T f1;
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN);
} else {
f0 = (real32_T)fabsf(u0);
f1 = (real32_T)fabsf(u1);
if (rtIsInfF(u1)) {
if (f0 == 1.0F) {
y = ((real32_T)rtNaN);
} else if (f0 > 1.0F) {
if (u1 > 0.0F) {
y = ((real32_T)rtInf);
} else {
y = 0.0F;
}
} else if (u1 > 0.0F) {
y = 0.0F;
} else {
y = ((real32_T)rtInf);
}
} else if (f1 == 0.0F) {
y = 1.0F;
} else if (f1 == 1.0F) {
if (u1 > 0.0F) {
y = u0;
} else {
y = 1.0F / u0;
}
} else if (u1 == 2.0F) {
y = u0 * u0;
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
y = (real32_T)sqrtf(u0);
} else if ((u0 < 0.0F) && (u1 > (real32_T)floorf(u1))) {
y = ((real32_T)rtNaN);
} else {
y = (real32_T)powf(u0, u1);
}
}
return y;
}
/*
*
*/
void power(const real32_T a[12], real_T b, real32_T y[12])
{
int32_T k;
for (k = 0; k < 12; k++) {
y[k] = rt_powf_snf(a[k], (real32_T)b);
}
}
/* End of code generation (power.c) */

14
apps/attitude_estimator_ekf/codegen/find.h → apps/attitude_estimator_ekf/codegen/power.h
View File

@ -1,14 +1,14 @@
/*
* find.h
* power.h
*
* Code generation for function 'find'
* Code generation for function 'power'
*
* C source code generated on: Fri Sep 21 13:56:43 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/
#ifndef __FIND_H__
#define __FIND_H__
#ifndef __POWER_H__
#define __POWER_H__
/* Include files */
#include <math.h>
#include <stdio.h>
@ -29,6 +29,6 @@
/* Variable Definitions */
/* Function Declarations */
extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]);
extern void power(const real32_T a[12], real_T b, real32_T y[12]);
#endif
/* End of code generation (find.h) */
/* End of code generation (power.h) */

2
apps/attitude_estimator_ekf/codegen/rtGetInf.c
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:45 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetInf.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:45 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetNaN.c
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:45 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetNaN.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_defines.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/

2
apps/attitude_estimator_ekf/codegen/rtwtypes.h
View File

@ -3,7 +3,7 @@
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Fri Sep 21 13:56:44 2012
* C source code generated on: Mon Oct 01 19:38:49 2012
*
*/