void sqrtm(const emxArray_real_T *A, emxArray_creal_T *X, real_T *arg2)
{
emxArray_creal_T *T;
emxArray_creal_T *y;
boolean_T b0;
const mxArray *b_y;
static const int32_T iv40[2] = { 1, 19 };
const mxArray *m2;
char_T cv12[19];
int32_T i;
static const char_T cv13[19] = { 'C', 'o', 'd', 'e', 'r', ':', 'M', 'A', 'T',
'L', 'A', 'B', ':', 's', 'q', 'u', 'a', 'r', 'e' };
emxArray_creal_T *Q;
emxArray_creal_T *r25;
int32_T loop_ub;
int16_T iv41[2];
emxArray_creal_T *R;
real_T s_re;
real_T s_im;
int32_T k;
real_T R_re;
real_T R_im;
real_T T_re;
real_T T_im;
real_T brm;
int32_T i1;
int32_T b_loop_ub;
int32_T i2;
emxArray_creal_T *b_T;
emxArray_real_T *b_X;
emlrtHeapReferenceStackEnterFcnR2012b(emlrtRootTLSGlobal);
emxInit_creal_T(&T, 2, &k_emlrtRTEI, TRUE);
emxInit_creal_T(&y, 2, &k_emlrtRTEI, TRUE);
b0 = (A->size[0] == A->size[1]);
if (b0) {
} else {
emlrtPushRtStackR2012b(&hb_emlrtRSI, emlrtRootTLSGlobal);
emlrt_synchGlobalsToML();
b_y = NULL;
m2 = mxCreateCharArray(2, iv40);
for (i = 0; i < 19; i++) {
cv12[i] = cv13[i];
}
emlrtInitCharArrayR2013a(emlrtRootTLSGlobal, 19, m2, cv12);
emlrtAssign(&b_y, m2);
error(message(b_y, &f_emlrtMCI), &g_emlrtMCI);
emlrt_synchGlobalsFromML();
emlrtPopRtStackR2012b(&hb_emlrtRSI, emlrtRootTLSGlobal);
}
emxInit_creal_T(&Q, 2, &k_emlrtRTEI, TRUE);
emxInit_creal_T(&r25, 2, &k_emlrtRTEI, TRUE);
emlrtPushRtStackR2012b(&ib_emlrtRSI, emlrtRootTLSGlobal);
schur(A, Q, r25);
i = T->size[0] * T->size[1];
T->size[0] = r25->size[0];
T->size[1] = r25->size[1];
emxEnsureCapacity((emxArray__common *)T, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
loop_ub = r25->size[0] * r25->size[1];
for (i = 0; i < loop_ub; i++) {
T->data[i] = r25->data[i];
}
emxFree_creal_T(&r25);
emlrtPopRtStackR2012b(&ib_emlrtRSI, emlrtRootTLSGlobal);
for (i = 0; i < 2; i++) {
iv41[i] = (int16_T)T->size[i];
}
emxInit_creal_T(&R, 2, &l_emlrtRTEI, TRUE);
i = R->size[0] * R->size[1];
R->size[0] = iv41[0];
emxEnsureCapacity((emxArray__common *)R, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
i = R->size[0] * R->size[1];
R->size[1] = iv41[1];
emxEnsureCapacity((emxArray__common *)R, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
loop_ub = iv41[0] * iv41[1];
for (i = 0; i < loop_ub; i++) {
R->data[i].re = 0.0;
R->data[i].im = 0.0;
}
emlrtPushRtStackR2012b(&jb_emlrtRSI, emlrtRootTLSGlobal);
b0 = isUTmatD(T);
emlrtPopRtStackR2012b(&jb_emlrtRSI, emlrtRootTLSGlobal);
if (b0) {
emlrtPushRtStackR2012b(&kb_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&kb_emlrtRSI, emlrtRootTLSGlobal);
for (loop_ub = 0; loop_ub + 1 <= A->size[0]; loop_ub++) {
R->data[loop_ub + R->size[0] * loop_ub] = T->data[loop_ub + T->size[0] *
loop_ub];
c_sqrt(&R->data[loop_ub + R->size[0] * loop_ub]);
}
} else {
emlrtPushRtStackR2012b(&lb_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&lb_emlrtRSI, emlrtRootTLSGlobal);
for (loop_ub = 0; loop_ub + 1 <= A->size[0]; loop_ub++) {
R->data[loop_ub + R->size[0] * loop_ub] = T->data[loop_ub + T->size[0] *
loop_ub];
c_sqrt(&R->data[loop_ub + R->size[0] * loop_ub]);
for (i = loop_ub - 1; i + 1 > 0; i--) {
s_re = 0.0;
s_im = 0.0;
emlrtPushRtStackR2012b(&mb_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&mb_emlrtRSI, emlrtRootTLSGlobal);
for (k = i + 1; k + 1 <= loop_ub; k++) {
R_re = R->data[i + R->size[0] * k].re * R->data[k + R->size[0] *
loop_ub].re - R->data[i + R->size[0] * k].im * R->data[k + R->size[0]
* loop_ub].im;
R_im = R->data[i + R->size[0] * k].re * R->data[k + R->size[0] *
loop_ub].im + R->data[i + R->size[0] * k].im * R->data[k + R->size[0]
* loop_ub].re;
s_re += R_re;
s_im += R_im;
}
T_re = T->data[i + T->size[0] * loop_ub].re - s_re;
T_im = T->data[i + T->size[0] * loop_ub].im - s_im;
R_re = R->data[i + R->size[0] * i].re + R->data[loop_ub + R->size[0] *
loop_ub].re;
R_im = R->data[i + R->size[0] * i].im + R->data[loop_ub + R->size[0] *
loop_ub].im;
if (R_im == 0.0) {
if (T_im == 0.0) {
R->data[i + R->size[0] * loop_ub].re = T_re / R_re;
R->data[i + R->size[0] * loop_ub].im = 0.0;
} else if (T_re == 0.0) {
R->data[i + R->size[0] * loop_ub].re = 0.0;
R->data[i + R->size[0] * loop_ub].im = T_im / R_re;
} else {
R->data[i + R->size[0] * loop_ub].re = T_re / R_re;
R->data[i + R->size[0] * loop_ub].im = T_im / R_re;
}
} else if (R_re == 0.0) {
if (T_re == 0.0) {
R->data[i + R->size[0] * loop_ub].re = T_im / R_im;
R->data[i + R->size[0] * loop_ub].im = 0.0;
} else if (T_im == 0.0) {
R->data[i + R->size[0] * loop_ub].re = 0.0;
R->data[i + R->size[0] * loop_ub].im = -(T_re / R_im);
} else {
R->data[i + R->size[0] * loop_ub].re = T_im / R_im;
R->data[i + R->size[0] * loop_ub].im = -(T_re / R_im);
}
} else {
brm = muDoubleScalarAbs(R_re);
s_re = muDoubleScalarAbs(R_im);
if (brm > s_re) {
s_re = R_im / R_re;
s_im = R_re + s_re * R_im;
R->data[i + R->size[0] * loop_ub].re = (T_re + s_re * T_im) / s_im;
R->data[i + R->size[0] * loop_ub].im = (T_im - s_re * T_re) / s_im;
} else if (s_re == brm) {
if (R_re > 0.0) {
s_im = 0.5;
} else {
s_im = -0.5;
}
if (R_im > 0.0) {
s_re = 0.5;
} else {
s_re = -0.5;
}
R->data[i + R->size[0] * loop_ub].re = (T_re * s_im + T_im * s_re) /
brm;
R->data[i + R->size[0] * loop_ub].im = (T_im * s_im - T_re * s_re) /
brm;
} else {
s_re = R_re / R_im;
s_im = R_im + s_re * R_re;
R->data[i + R->size[0] * loop_ub].re = (s_re * T_re + T_im) / s_im;
R->data[i + R->size[0] * loop_ub].im = (s_re * T_im - T_re) / s_im;
}
}
}
}
}
emlrtPushRtStackR2012b(&nb_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&kh_emlrtRSI, emlrtRootTLSGlobal);
dynamic_size_checks(Q, R);
emlrtPopRtStackR2012b(&kh_emlrtRSI, emlrtRootTLSGlobal);
if ((Q->size[1] == 1) || (R->size[0] == 1)) {
i = y->size[0] * y->size[1];
y->size[0] = Q->size[0];
y->size[1] = R->size[1];
emxEnsureCapacity((emxArray__common *)y, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
loop_ub = Q->size[0];
for (i = 0; i < loop_ub; i++) {
k = R->size[1];
for (i1 = 0; i1 < k; i1++) {
y->data[i + y->size[0] * i1].re = 0.0;
y->data[i + y->size[0] * i1].im = 0.0;
b_loop_ub = Q->size[1];
for (i2 = 0; i2 < b_loop_ub; i2++) {
s_re = Q->data[i + Q->size[0] * i2].re * R->data[i2 + R->size[0] * i1]
.re - Q->data[i + Q->size[0] * i2].im * R->data[i2 + R->size[0] * i1]
.im;
s_im = Q->data[i + Q->size[0] * i2].re * R->data[i2 + R->size[0] * i1]
.im + Q->data[i + Q->size[0] * i2].im * R->data[i2 + R->size[0] * i1]
.re;
y->data[i + y->size[0] * i1].re += s_re;
y->data[i + y->size[0] * i1].im += s_im;
}
}
}
} else {
iv41[0] = (int16_T)Q->size[0];
iv41[1] = (int16_T)R->size[1];
emlrtPushRtStackR2012b(&jh_emlrtRSI, emlrtRootTLSGlobal);
i = y->size[0] * y->size[1];
y->size[0] = iv41[0];
emxEnsureCapacity((emxArray__common *)y, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
i = y->size[0] * y->size[1];
y->size[1] = iv41[1];
emxEnsureCapacity((emxArray__common *)y, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
loop_ub = iv41[0] * iv41[1];
for (i = 0; i < loop_ub; i++) {
y->data[i].re = 0.0;
y->data[i].im = 0.0;
}
eml_xgemm(Q->size[0], R->size[1], Q->size[1], Q, Q->size[0], R, Q->size[1],
y, Q->size[0]);
emlrtPopRtStackR2012b(&jh_emlrtRSI, emlrtRootTLSGlobal);
}
emxFree_creal_T(&R);
i = T->size[0] * T->size[1];
T->size[0] = Q->size[1];
T->size[1] = Q->size[0];
emxEnsureCapacity((emxArray__common *)T, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
loop_ub = Q->size[0];
for (i = 0; i < loop_ub; i++) {
k = Q->size[1];
for (i1 = 0; i1 < k; i1++) {
T->data[i1 + T->size[0] * i].re = Q->data[i + Q->size[0] * i1].re;
T->data[i1 + T->size[0] * i].im = -Q->data[i + Q->size[0] * i1].im;
}
}
emxFree_creal_T(&Q);
emlrtPushRtStackR2012b(&kh_emlrtRSI, emlrtRootTLSGlobal);
dynamic_size_checks(y, T);
emlrtPopRtStackR2012b(&kh_emlrtRSI, emlrtRootTLSGlobal);
if ((y->size[1] == 1) || (T->size[0] == 1)) {
i = X->size[0] * X->size[1];
X->size[0] = y->size[0];
X->size[1] = T->size[1];
emxEnsureCapacity((emxArray__common *)X, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
loop_ub = y->size[0];
for (i = 0; i < loop_ub; i++) {
k = T->size[1];
for (i1 = 0; i1 < k; i1++) {
X->data[i + X->size[0] * i1].re = 0.0;
X->data[i + X->size[0] * i1].im = 0.0;
b_loop_ub = y->size[1];
for (i2 = 0; i2 < b_loop_ub; i2++) {
s_re = y->data[i + y->size[0] * i2].re * T->data[i2 + T->size[0] * i1]
.re - y->data[i + y->size[0] * i2].im * T->data[i2 + T->size[0] * i1]
.im;
s_im = y->data[i + y->size[0] * i2].re * T->data[i2 + T->size[0] * i1]
.im + y->data[i + y->size[0] * i2].im * T->data[i2 + T->size[0] * i1]
.re;
X->data[i + X->size[0] * i1].re += s_re;
X->data[i + X->size[0] * i1].im += s_im;
}
}
}
} else {
iv41[0] = (int16_T)y->size[0];
iv41[1] = (int16_T)T->size[1];
emlrtPushRtStackR2012b(&jh_emlrtRSI, emlrtRootTLSGlobal);
i = X->size[0] * X->size[1];
X->size[0] = iv41[0];
emxEnsureCapacity((emxArray__common *)X, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
i = X->size[0] * X->size[1];
X->size[1] = iv41[1];
emxEnsureCapacity((emxArray__common *)X, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
loop_ub = iv41[0] * iv41[1];
for (i = 0; i < loop_ub; i++) {
X->data[i].re = 0.0;
X->data[i].im = 0.0;
}
eml_xgemm(y->size[0], T->size[1], y->size[1], y, y->size[0], T, y->size[1],
X, y->size[0]);
emlrtPopRtStackR2012b(&jh_emlrtRSI, emlrtRootTLSGlobal);
}
emlrtPopRtStackR2012b(&nb_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&ob_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&kh_emlrtRSI, emlrtRootTLSGlobal);
dynamic_size_checks(X, X);
emlrtPopRtStackR2012b(&kh_emlrtRSI, emlrtRootTLSGlobal);
if ((X->size[1] == 1) || (X->size[0] == 1)) {
i = T->size[0] * T->size[1];
T->size[0] = X->size[0];
T->size[1] = X->size[1];
emxEnsureCapacity((emxArray__common *)T, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
loop_ub = X->size[0];
for (i = 0; i < loop_ub; i++) {
k = X->size[1];
for (i1 = 0; i1 < k; i1++) {
T->data[i + T->size[0] * i1].re = 0.0;
T->data[i + T->size[0] * i1].im = 0.0;
b_loop_ub = X->size[1];
for (i2 = 0; i2 < b_loop_ub; i2++) {
s_re = X->data[i + X->size[0] * i2].re * X->data[i2 + X->size[0] * i1]
.re - X->data[i + X->size[0] * i2].im * X->data[i2 + X->size[0] * i1]
.im;
s_im = X->data[i + X->size[0] * i2].re * X->data[i2 + X->size[0] * i1]
.im + X->data[i + X->size[0] * i2].im * X->data[i2 + X->size[0] * i1]
.re;
T->data[i + T->size[0] * i1].re += s_re;
T->data[i + T->size[0] * i1].im += s_im;
}
}
}
} else {
iv41[0] = (int16_T)X->size[0];
iv41[1] = (int16_T)X->size[1];
emlrtPushRtStackR2012b(&jh_emlrtRSI, emlrtRootTLSGlobal);
i = T->size[0] * T->size[1];
T->size[0] = iv41[0];
emxEnsureCapacity((emxArray__common *)T, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
i = T->size[0] * T->size[1];
T->size[1] = iv41[1];
emxEnsureCapacity((emxArray__common *)T, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
loop_ub = iv41[0] * iv41[1];
for (i = 0; i < loop_ub; i++) {
T->data[i].re = 0.0;
T->data[i].im = 0.0;
}
eml_xgemm(X->size[0], X->size[1], X->size[1], X, X->size[0], X, X->size[1],
T, X->size[0]);
emlrtPopRtStackR2012b(&jh_emlrtRSI, emlrtRootTLSGlobal);
}
emxInit_creal_T(&b_T, 2, &k_emlrtRTEI, TRUE);
emlrtPopRtStackR2012b(&ob_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&ob_emlrtRSI, emlrtRootTLSGlobal);
i = b_T->size[0] * b_T->size[1];
b_T->size[0] = T->size[0];
b_T->size[1] = T->size[1];
emxEnsureCapacity((emxArray__common *)b_T, i, (int32_T)sizeof(creal_T),
&k_emlrtRTEI);
loop_ub = T->size[0] * T->size[1];
for (i = 0; i < loop_ub; i++) {
b_T->data[i].re = T->data[i].re - A->data[i];
b_T->data[i].im = T->data[i].im;
}
emxInit_real_T(&b_X, 2, &k_emlrtRTEI, TRUE);
s_re = norm(b_T);
s_im = b_norm(A);
*arg2 = s_re / s_im;
emlrtPopRtStackR2012b(&ob_emlrtRSI, emlrtRootTLSGlobal);
i = b_X->size[0] * b_X->size[1];
b_X->size[0] = X->size[0];
b_X->size[1] = X->size[1];
emxEnsureCapacity((emxArray__common *)b_X, i, (int32_T)sizeof(real_T),
&k_emlrtRTEI);
loop_ub = X->size[0] * X->size[1];
emxFree_creal_T(&b_T);
for (i = 0; i < loop_ub; i++) {
b_X->data[i] = X->data[i].im;
}
if (c_norm(b_X) <= 10.0 * (real_T)A->size[0] * 2.2204460492503131E-16 * d_norm
(X)) {
emlrtPushRtStackR2012b(&pb_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&pb_emlrtRSI, emlrtRootTLSGlobal);
for (loop_ub = 0; loop_ub + 1 <= A->size[0]; loop_ub++) {
emlrtPushRtStackR2012b(&qb_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&qb_emlrtRSI, emlrtRootTLSGlobal);
for (i = 0; i + 1 <= A->size[0]; i++) {
s_re = X->data[i + X->size[0] * loop_ub].re;
X->data[i + X->size[0] * loop_ub].re = s_re;
X->data[i + X->size[0] * loop_ub].im = 0.0;
}
}
}
emxFree_real_T(&b_X);
emxFree_creal_T(&y);
emxFree_creal_T(&T);
emlrtHeapReferenceStackLeaveFcnR2012b(emlrtRootTLSGlobal);
}
/* Function Definitions */
void magCali(const real_T data[6000], creal_T A_i[9], creal_T B[3])
{
real_T D[20000];
int32_T i;
int32_T i0;
real_T S[100];
int32_T ind;
real_T L_S22[16];
real_T L_S22_i[16];
real_T b_L_S22[16];
real_T S22_i[16];
real_T A[36];
real_T b_S[24];
real_T c_S[36];
real_T norm_v1;
static const real_T b_A[36] = { 0.0, 0.5, 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.5,
0.0, 0.0, 0.0, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, -0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25 };
creal_T L[36];
creal_T V[36];
creal_T b_max;
creal_T v1[6];
real_T norm_VV2;
creal_T b_S22_i[24];
creal_T v2[4];
creal_T v[100];
creal_T Q[9];
creal_T L_Q[9];
creal_T L_Q_i[9];
creal_T b_L_Q[9];
creal_T b_v[3];
creal_T QB[3];
creal_T b_Q[9];
real_T norm_VV3;
creal_T b_L_Q_i[9];
real_T ar;
real_T ai;
/* magnetometer calibration */
/* */
/* Jungtaek Kim */
/* [email protected] */
/* */
memset(&D[0], 0, 20000U * sizeof(real_T));
for (i = 0; i < 2000; i++) {
D[i] = data[i] * data[i];
D[2000 + i] = data[2000 + i] * data[2000 + i];
D[4000 + i] = data[4000 + i] * data[4000 + i];
D[6000 + i] = 2.0 * data[2000 + i] * data[4000 + i];
D[8000 + i] = 2.0 * data[i] * data[4000 + i];
D[10000 + i] = 2.0 * data[i] * data[2000 + i];
D[12000 + i] = 2.0 * data[i];
D[14000 + i] = 2.0 * data[2000 + i];
D[16000 + i] = 2.0 * data[4000 + i];
D[18000 + i] = 1.0;
}
for (i0 = 0; i0 < 10; i0++) {
for (i = 0; i < 10; i++) {
S[i0 + 10 * i] = 0.0;
for (ind = 0; ind < 2000; ind++) {
S[i0 + 10 * i] += D[ind + 2000 * i0] * D[ind + 2000 * i];
}
}
}
for (i0 = 0; i0 < 4; i0++) {
for (i = 0; i < 4; i++) {
L_S22[i + (i0 << 2)] = S[(i + 10 * (6 + i0)) + 6];
}
}
chol(L_S22);
inv(L_S22, L_S22_i);
for (i0 = 0; i0 < 4; i0++) {
for (i = 0; i < 4; i++) {
b_L_S22[i + (i0 << 2)] = L_S22[i0 + (i << 2)];
}
}
inv(b_L_S22, L_S22);
for (i0 = 0; i0 < 4; i0++) {
for (i = 0; i < 4; i++) {
S22_i[i0 + (i << 2)] = 0.0;
for (ind = 0; ind < 4; ind++) {
S22_i[i0 + (i << 2)] += L_S22[i0 + (ind << 2)] * L_S22_i[ind + (i << 2)];
}
}
}
/* S22_i = pinv(S22);%I4\S22; */
for (i0 = 0; i0 < 6; i0++) {
for (i = 0; i < 4; i++) {
b_S[i0 + 6 * i] = 0.0;
for (ind = 0; ind < 4; ind++) {
b_S[i0 + 6 * i] += S[i0 + 10 * (6 + ind)] * S22_i[ind + (i << 2)];
}
}
}
for (i0 = 0; i0 < 6; i0++) {
for (i = 0; i < 6; i++) {
norm_v1 = 0.0;
for (ind = 0; ind < 4; ind++) {
norm_v1 += b_S[i0 + 6 * ind] * S[(ind + 10 * i) + 6];
}
c_S[i0 + 6 * i] = S[i0 + 10 * i] - norm_v1;
}
}
for (i0 = 0; i0 < 6; i0++) {
for (i = 0; i < 6; i++) {
A[i0 + 6 * i] = 0.0;
for (ind = 0; ind < 6; ind++) {
A[i0 + 6 * i] += b_A[i0 + 6 * ind] * c_S[ind + 6 * i];
}
}
}
eig(A, V, L);
b_max = L[0];
ind = 0;
for (i = 0; i < 6; i++) {
if (b_max.re < L[i + 6 * i].re) {
b_max = L[i + 6 * i];
ind = i;
}
}
memcpy(&v1[0], &V[6 * ind], 6U * sizeof(creal_T));
if (V[6 * ind].re < 0.0) {
for (i0 = 0; i0 < 6; i0++) {
v1[i0].re = -V[i0 + 6 * ind].re;
v1[i0].im = -V[i0 + 6 * ind].im;
}
}
norm_v1 = norm(v1);
for (i0 = 0; i0 < 6; i0++) {
norm_VV2 = v1[i0].im;
if (v1[i0].im == 0.0) {
v1[i0].re /= norm_v1;
v1[i0].im = 0.0;
} else if (v1[i0].re == 0.0) {
v1[i0].re = 0.0;
v1[i0].im = norm_VV2 / norm_v1;
} else {
v1[i0].re /= norm_v1;
v1[i0].im = norm_VV2 / norm_v1;
}
}
for (i0 = 0; i0 < 4; i0++) {
for (i = 0; i < 6; i++) {
norm_v1 = 0.0;
for (ind = 0; ind < 4; ind++) {
norm_v1 += S22_i[i0 + (ind << 2)] * S[(ind + 10 * i) + 6];
}
b_S22_i[i0 + (i << 2)].re = norm_v1;
b_S22_i[i0 + (i << 2)].im = 0.0;
}
}
for (i0 = 0; i0 < 4; i0++) {
v2[i0].re = 0.0;
v2[i0].im = 0.0;
for (i = 0; i < 6; i++) {
v2[i0].re += b_S22_i[i0 + (i << 2)].re * v1[i].re - 0.0 * v1[i].im;
v2[i0].im += b_S22_i[i0 + (i << 2)].re * v1[i].im + 0.0 * v1[i].re;
}
}
for (i0 = 0; i0 < 100; i0++) {
v[i0].re = 0.0;
v[i0].im = 0.0;
}
v[0] = v1[0];
v[1] = v1[1];
v[2] = v1[2];
v[3] = v1[3];
v[4] = v1[4];
v[5] = v1[5];
v[6].re = -v2[0].re;
v[6].im = -v2[0].im;
v[7].re = -v2[1].re;
v[7].im = -v2[1].im;
v[8].re = -v2[2].re;
v[8].im = -v2[2].im;
v[9].re = -v2[3].re;
v[9].im = -v2[3].im;
Q[0] = v[0];
Q[3] = v[5];
Q[6] = v[4];
Q[1] = v[5];
Q[4] = v[1];
Q[7] = v[3];
Q[2] = v[4];
Q[5] = v[3];
Q[8] = v[2];
memcpy(&L_Q[0], &Q[0], 9U * sizeof(creal_T));
b_chol(L_Q);
b_inv(L_Q, L_Q_i);
for (i0 = 0; i0 < 3; i0++) {
for (i = 0; i < 3; i++) {
b_L_Q[i + 3 * i0].re = L_Q[i0 + 3 * i].re;
b_L_Q[i + 3 * i0].im = -L_Q[i0 + 3 * i].im;
}
}
b_inv(b_L_Q, L_Q);
/* Q_i = pinv(Q);%I3\Q; */
for (i0 = 0; i0 < 3; i0++) {
for (i = 0; i < 3; i++) {
b_L_Q[i0 + 3 * i].re = 0.0;
b_L_Q[i0 + 3 * i].im = 0.0;
for (ind = 0; ind < 3; ind++) {
b_L_Q[i0 + 3 * i].re += L_Q[i0 + 3 * ind].re * L_Q_i[ind + 3 * i].re -
L_Q[i0 + 3 * ind].im * L_Q_i[ind + 3 * i].im;
b_L_Q[i0 + 3 * i].im += L_Q[i0 + 3 * ind].re * L_Q_i[ind + 3 * i].im +
L_Q[i0 + 3 * ind].im * L_Q_i[ind + 3 * i].re;
}
}
}
b_v[0] = v[6];
b_v[1] = v[7];
b_v[2] = v[8];
for (i0 = 0; i0 < 3; i0++) {
norm_v1 = 0.0;
norm_VV2 = 0.0;
for (i = 0; i < 3; i++) {
norm_v1 += b_L_Q[i0 + 3 * i].re * b_v[i].re - b_L_Q[i0 + 3 * i].im * b_v[i]
.im;
norm_VV2 += b_L_Q[i0 + 3 * i].re * b_v[i].im + b_L_Q[i0 + 3 * i].im *
b_v[i].re;
}
B[i0].re = -norm_v1;
B[i0].im = -norm_VV2;
}
for (i0 = 0; i0 < 3; i0++) {
QB[i0].re = 0.0;
QB[i0].im = 0.0;
for (i = 0; i < 3; i++) {
QB[i0].re += Q[i0 + 3 * i].re * B[i].re - Q[i0 + 3 * i].im * B[i].im;
QB[i0].im += Q[i0 + 3 * i].re * B[i].im + Q[i0 + 3 * i].im * B[i].re;
}
b_v[i0].re = B[i0].re;
b_v[i0].im = -B[i0].im;
}
b_max.re = 0.0;
b_max.im = 0.0;
for (i = 0; i < 3; i++) {
b_max.re += b_v[i].re * QB[i].re - b_v[i].im * QB[i].im;
b_max.im += b_v[i].re * QB[i].im + b_v[i].im * QB[i].re;
}
b_max.re -= v[9].re;
b_max.im -= v[9].im;
b_sqrt(&b_max);
memcpy(&b_Q[0], &Q[0], 9U * sizeof(creal_T));
b_eig(b_Q, L_Q, Q);
norm_v1 = b_norm(*(creal_T (*)[3])&L_Q[0]);
norm_VV2 = b_norm(*(creal_T (*)[3])&L_Q[3]);
norm_VV3 = b_norm(*(creal_T (*)[3])&L_Q[6]);
for (i0 = 0; i0 < 3; i0++) {
if (L_Q[i0].im == 0.0) {
L_Q_i[i0].re = L_Q[i0].re / norm_v1;
L_Q_i[i0].im = 0.0;
} else if (L_Q[i0].re == 0.0) {
L_Q_i[i0].re = 0.0;
L_Q_i[i0].im = L_Q[i0].im / norm_v1;
} else {
L_Q_i[i0].re = L_Q[i0].re / norm_v1;
L_Q_i[i0].im = L_Q[i0].im / norm_v1;
}
if (L_Q[3 + i0].im == 0.0) {
L_Q_i[3 + i0].re = L_Q[3 + i0].re / norm_VV2;
L_Q_i[3 + i0].im = 0.0;
} else if (L_Q[3 + i0].re == 0.0) {
L_Q_i[3 + i0].re = 0.0;
L_Q_i[3 + i0].im = L_Q[3 + i0].im / norm_VV2;
} else {
L_Q_i[3 + i0].re = L_Q[3 + i0].re / norm_VV2;
L_Q_i[3 + i0].im = L_Q[3 + i0].im / norm_VV2;
}
if (L_Q[6 + i0].im == 0.0) {
L_Q_i[6 + i0].re = L_Q[6 + i0].re / norm_VV3;
L_Q_i[6 + i0].im = 0.0;
} else if (L_Q[6 + i0].re == 0.0) {
L_Q_i[6 + i0].re = 0.0;
L_Q_i[6 + i0].im = L_Q[6 + i0].im / norm_VV3;
} else {
L_Q_i[6 + i0].re = L_Q[6 + i0].re / norm_VV3;
L_Q_i[6 + i0].im = L_Q[6 + i0].im / norm_VV3;
}
for (i = 0; i < 3; i++) {
b_L_Q[i0 + 3 * i].re = 0.0;
b_L_Q[i0 + 3 * i].im = 0.0;
for (ind = 0; ind < 3; ind++) {
b_L_Q[i0 + 3 * i].re += L_Q_i[i0 + 3 * ind].re * Q[ind + 3 * i].re -
L_Q_i[i0 + 3 * ind].im * Q[ind + 3 * i].im;
b_L_Q[i0 + 3 * i].im += L_Q_i[i0 + 3 * ind].re * Q[ind + 3 * i].im +
L_Q_i[i0 + 3 * ind].im * Q[ind + 3 * i].re;
}
}
}
for (i0 = 0; i0 < 3; i0++) {
for (i = 0; i < 3; i++) {
b_L_Q_i[i0 + 3 * i].re = 0.0;
b_L_Q_i[i0 + 3 * i].im = 0.0;
for (ind = 0; ind < 3; ind++) {
b_L_Q_i[i0 + 3 * i].re += b_L_Q[i0 + 3 * ind].re * L_Q_i[i + 3 * ind].re
- b_L_Q[i0 + 3 * ind].im * -L_Q_i[i + 3 * ind].im;
b_L_Q_i[i0 + 3 * i].im += b_L_Q[i0 + 3 * ind].re * -L_Q_i[i + 3 * ind].
im + b_L_Q[i0 + 3 * ind].im * L_Q_i[i + 3 * ind].re;
}
}
}
for (i0 = 0; i0 < 3; i0++) {
for (i = 0; i < 3; i++) {
ar = b_L_Q_i[i + 3 * i0].re * 0.569;
ai = b_L_Q_i[i + 3 * i0].im * 0.569;
if (b_max.im == 0.0) {
if (ai == 0.0) {
A_i[i + 3 * i0].re = ar / b_max.re;
A_i[i + 3 * i0].im = 0.0;
} else if (ar == 0.0) {
A_i[i + 3 * i0].re = 0.0;
A_i[i + 3 * i0].im = ai / b_max.re;
} else {
A_i[i + 3 * i0].re = ar / b_max.re;
A_i[i + 3 * i0].im = ai / b_max.re;
}
} else if (b_max.re == 0.0) {
if (ar == 0.0) {
A_i[i + 3 * i0].re = ai / b_max.im;
A_i[i + 3 * i0].im = 0.0;
} else if (ai == 0.0) {
A_i[i + 3 * i0].re = 0.0;
A_i[i + 3 * i0].im = -(ar / b_max.im);
} else {
A_i[i + 3 * i0].re = ai / b_max.im;
A_i[i + 3 * i0].im = -(ar / b_max.im);
}
} else {
norm_VV3 = fabs(b_max.re);
norm_v1 = fabs(b_max.im);
if (norm_VV3 > norm_v1) {
norm_v1 = b_max.im / b_max.re;
norm_VV2 = b_max.re + norm_v1 * b_max.im;
A_i[i + 3 * i0].re = (ar + norm_v1 * ai) / norm_VV2;
A_i[i + 3 * i0].im = (ai - norm_v1 * ar) / norm_VV2;
} else if (norm_v1 == norm_VV3) {
norm_v1 = b_max.re > 0.0 ? 0.5 : -0.5;
norm_VV2 = b_max.im > 0.0 ? 0.5 : -0.5;
A_i[i + 3 * i0].re = (ar * norm_v1 + ai * norm_VV2) / norm_VV3;
A_i[i + 3 * i0].im = (ai * norm_v1 - ar * norm_VV2) / norm_VV3;
} else {
norm_v1 = b_max.re / b_max.im;
norm_VV2 = b_max.im + norm_v1 * b_max.re;
A_i[i + 3 * i0].re = (norm_v1 * ar + ai) / norm_VV2;
A_i[i + 3 * i0].im = (norm_v1 * ai - ar) / norm_VV2;
}
}
}
}
}
