1145 if (!
ukf_DW.P_not_empty) {
1153 while ((!exitg1) && (b_j < 8)) {
1154 jj = (colj + b_j) - 1;
1156 if (!(b_j - 1 < 1)) {
1159 for (i = 1; i < b_j; i++) {
1160 lambda += SQ[ix] * SQ[iy];
1173 ix = ((6 - b_j) * 7 + colj) + 8;
1174 for (iy = colj + 8; iy <= ix; iy += 7) {
1178 for (ia = iy; ia <= e; ia++) {
1179 lambda += SQ[ia - 1] * SQ[b_ix];
1189 ix = ((6 - b_j) * 7 + jj) + 8;
1190 for (b_iy = jj + 7; b_iy + 1 <= ix; b_iy += 7) {
1211 for (b_j = 0; b_j + 1 <= jmax; b_j++) {
1212 for (colj = b_j + 1; colj + 1 <= jmax; colj++) {
1213 SQ[colj + 7 * b_j] = 0.0F;
1217 for (jmax = 0; jmax < 7; jmax++) {
1218 for (b_j = 0; b_j < 7; b_j++) {
1219 rtb_P1[b_j + 7 * jmax] = SQ[7 * b_j + jmax];
1223 ukf_DW.P_not_empty =
true;
1226 memcpy(&rtb_P1[0], &
ukf_DW.Delay_DSTATE[0], 49U *
sizeof(
real32_T));
1229 if (!
ukf_DW.x_not_empty) {
1232 for (i = 0; i < 7; i++) {
1236 ukf_DW.x_not_empty =
true;
1239 for (i = 0; i < 7; i++) {
1240 rtb_x1[i] =
ukf_DW.Delay1_DSTATE[i];
1261 ajj = 0.5F / (7.0F +
lambda);
1263 for (i = 0; i < 14; i++) {
1264 rtb_Wm[i + 1] = ajj;
1268 for (i = 0; i < 15; i++) {
1269 rtb_Wc[i] = rtb_Wm[i];
1295 for (jmax = 0; jmax < 49; jmax++) {
1296 SQ[jmax] =
lambda * rtb_P1[jmax];
1303 for (jmax = 0; jmax < 7; jmax++) {
1304 for (b_j = 0; b_j < 7; b_j++) {
1305 Y[b_j + 7 * jmax] = rtb_x1[b_j];
1308 rtb_X[jmax] = rtb_x1[jmax];
1311 for (jmax = 0; jmax < 49; jmax++) {
1312 rtb_X[jmax + 7] = Y[jmax] + SQ[jmax];
1313 rtb_X[jmax + 56] = Y[jmax] - SQ[jmax];
1350 for (i = 0; i < 7; i++) {
1355 memset(&rtb_Y[0], 0, 105U *
sizeof(
real32_T));
1358 u[0] =
ukf_U.rates[0];
1359 u[3] =
ukf_U.accel[0];
1360 u[1] =
ukf_U.rates[1];
1361 u[4] =
ukf_U.accel[1];
1362 u[2] =
ukf_U.rates[2];
1363 u[5] =
ukf_U.accel[2];
1366 for (i = 0; i < 15; i++) {
1373 for (jmax = 0; jmax < 7; jmax++) {
1374 rtb_x1[jmax] += rtb_Y[7 * i + jmax] * rtb_Wm[i];
1383 for (b_iy = 0; b_iy < 49; b_iy++) {
1389 lambda = sqrtf(rtb_Wm[1]);
1394 for (jmax = 0; jmax < 7; jmax++) {
1395 for (b_j = 0; b_j < 14; b_j++) {
1396 lambda_1[b_j + 21 * jmax] = (rtb_Y[(1 + b_j) * 7 + jmax] - rtb_x1[jmax]) *
1401 for (jmax = 0; jmax < 7; jmax++) {
1402 for (b_j = 0; b_j < 7; b_j++) {
1403 lambda_1[(b_j + 21 * jmax) + 14] = SQ[7 * b_j + jmax];
1407 qr_e(lambda_1, unusedU0, SQ);
1410 for (jmax = 0; jmax < 7; jmax++) {
1411 p[jmax] = rtb_Y[jmax] - rtb_x1[jmax];
1414 if (rtb_Wc[0] < 0.0F) {
1417 lambda = powf(-rtb_Wc[0], 0.25F);
1418 ajj = powf(-rtb_Wc[0], 0.25F);
1419 for (jmax = 0; jmax < 7; jmax++) {
1420 for (b_j = 0; b_j < 7; b_j++) {
1421 Y[jmax + 7 * b_j] = 0.0F;
1422 for (colj = 0; colj < 7; colj++) {
1423 Y[jmax + 7 * b_j] += SQ[7 * jmax + colj] * SQ[7 * b_j + colj];
1426 lambda_2[jmax + 7 * b_j] =
lambda *
p[jmax] * (ajj *
p[b_j]);
1430 for (jmax = 0; jmax < 7; jmax++) {
1431 for (b_j = 0; b_j < 7; b_j++) {
1432 rtb_P1[b_j + 7 * jmax] = Y[7 * jmax + b_j] - lambda_2[7 * jmax + b_j];
1440 while ((!exitg1) && (jmax < 8)) {
1441 colj = (i + jmax) - 1;
1443 if (!(jmax - 1 < 1)) {
1446 for (ix = 1; ix < jmax; ix++) {
1447 lambda += rtb_P1[jj] * rtb_P1[b_iy];
1458 if (jmax - 1 != 0) {
1460 b_iy = ((6 - jmax) * 7 + i) + 8;
1461 for (ix = i + 8; ix <= b_iy; ix += 7) {
1464 b_ix = (ix + jmax) - 2;
1465 for (e = ix; e <= b_ix; e++) {
1466 ajj += rtb_P1[e - 1] * rtb_P1[iy];
1476 jj = ((6 - jmax) * 7 + colj) + 8;
1477 for (colj += 7; colj + 1 <= jj; colj += 7) {
1498 for (b_j = 0; b_j + 1 <= i; b_j++) {
1499 for (jmax = b_j + 1; jmax + 1 <= i; jmax++) {
1500 rtb_P1[jmax + 7 * b_j] = 0.0F;
1505 lambda = powf(rtb_Wc[0], 0.25F);
1506 ajj = powf(rtb_Wc[0], 0.25F);
1507 for (jmax = 0; jmax < 7; jmax++) {
1508 for (b_j = 0; b_j < 7; b_j++) {
1509 Y[jmax + 7 * b_j] = 0.0F;
1510 for (colj = 0; colj < 7; colj++) {
1511 Y[jmax + 7 * b_j] += SQ[7 * jmax + colj] * SQ[7 * b_j + colj];
1514 lambda_2[jmax + 7 * b_j] =
lambda *
p[jmax] * (ajj *
p[b_j]);
1518 for (jmax = 0; jmax < 7; jmax++) {
1519 for (b_j = 0; b_j < 7; b_j++) {
1520 rtb_P1[b_j + 7 * jmax] = Y[7 * jmax + b_j] + lambda_2[7 * jmax + b_j];
1528 while ((!exitg1) && (b_j < 8)) {
1529 jj = (colj + b_j) - 1;
1531 if (!(b_j - 1 < 1)) {
1534 for (b_iy = 1; b_iy < b_j; b_iy++) {
1535 lambda += rtb_P1[ix] * rtb_P1[iy];
1541 ajj = rtb_P1[jj] -
lambda;
1548 ix = ((6 - b_j) * 7 + colj) + 8;
1549 for (iy = colj + 8; iy <= ix; iy += 7) {
1553 for (ia = iy; ia <= e; ia++) {
1554 lambda += rtb_P1[ia - 1] * rtb_P1[b_ix];
1564 ix = ((6 - b_j) * 7 + jj) + 8;
1565 for (i = jj + 7; i + 1 <= ix; i += 7) {
1586 for (b_j = 0; b_j + 1 <= jmax; b_j++) {
1587 for (colj = b_j + 1; colj + 1 <= jmax; colj++) {
1588 rtb_P1[colj + 7 * b_j] = 0.0F;
1627 for (i = 0; i < 6; i++) {
1635 for (i = 0; i < 15; i++) {
1639 h(&rtb_Y[7 * i],
ukf_U.q, u);
1640 for (jmax = 0; jmax < 6; jmax++) {
1641 rtb_Z[jmax + 6 * i] = u[jmax];
1646 for (jmax = 0; jmax < 6; jmax++) {
1647 rtb_z[jmax] += rtb_Z[6 * i + jmax] * rtb_Wm[i];
1656 for (b_iy = 0; b_iy < 36; b_iy++) {
1662 lambda = sqrtf(rtb_Wm[1]);
1669 for (jmax = 0; jmax < 6; jmax++) {
1670 for (b_j = 0; b_j < 14; b_j++) {
1671 lambda_0[b_j + 20 * jmax] = (rtb_Z[(1 + b_j) * 6 + jmax] - rtb_z[jmax]) *
1676 for (jmax = 0; jmax < 6; jmax++) {
1677 for (b_j = 0; b_j < 6; b_j++) {
1678 lambda_0[(b_j + 20 * jmax) + 14] = SR[6 * b_j + jmax];
1682 qr(lambda_0, unusedU0_0, SR);
1686 for (jmax = 0; jmax < 6; jmax++) {
1687 u[jmax] = rtb_Z[jmax] - rtb_z[jmax];
1691 if (rtb_Wc[0] < 0.0F) {
1694 lambda = powf(-rtb_Wc[0], 0.25F);
1695 ajj = powf(-rtb_Wc[0], 0.25F);
1696 for (jmax = 0; jmax < 6; jmax++) {
1697 for (b_j = 0; b_j < 6; b_j++) {
1698 SR_0[jmax + 6 * b_j] = 0.0F;
1699 for (colj = 0; colj < 6; colj++) {
1700 SR_0[jmax + 6 * b_j] += SR[6 * jmax + colj] * SR[6 * b_j + colj];
1703 lambda_3[jmax + 6 * b_j] =
lambda * u[jmax] * (ajj * u[b_j]);
1707 for (jmax = 0; jmax < 6; jmax++) {
1708 for (b_j = 0; b_j < 6; b_j++) {
1709 SR[b_j + 6 * jmax] = SR_0[6 * jmax + b_j] - lambda_3[6 * jmax + b_j];
1717 while ((!exitg1) && (jmax < 7)) {
1718 colj = (i + jmax) - 1;
1720 if (!(jmax - 1 < 1)) {
1723 for (ix = 1; ix < jmax; ix++) {
1724 lambda += SR[jj] * SR[b_iy];
1735 if (jmax - 1 != 0) {
1737 b_iy = ((5 - jmax) * 6 + i) + 7;
1738 for (ix = i + 7; ix <= b_iy; ix += 6) {
1741 b_ix = (ix + jmax) - 2;
1742 for (e = ix; e <= b_ix; e++) {
1743 ajj += SR[e - 1] * SR[iy];
1753 jj = ((5 - jmax) * 6 + colj) + 7;
1754 for (colj += 6; colj + 1 <= jj; colj += 6) {
1775 for (b_j = 0; b_j + 1 <= i; b_j++) {
1776 for (jmax = b_j + 1; jmax + 1 <= i; jmax++) {
1777 SR[jmax + 6 * b_j] = 0.0F;
1782 lambda = powf(rtb_Wc[0], 0.25F);
1783 ajj = powf(rtb_Wc[0], 0.25F);
1784 for (jmax = 0; jmax < 6; jmax++) {
1785 for (b_j = 0; b_j < 6; b_j++) {
1786 SR_0[jmax + 6 * b_j] = 0.0F;
1787 for (colj = 0; colj < 6; colj++) {
1788 SR_0[jmax + 6 * b_j] += SR[6 * jmax + colj] * SR[6 * b_j + colj];
1791 lambda_3[jmax + 6 * b_j] =
lambda * u[jmax] * (ajj * u[b_j]);
1795 for (jmax = 0; jmax < 6; jmax++) {
1796 for (b_j = 0; b_j < 6; b_j++) {
1797 SR[b_j + 6 * jmax] = SR_0[6 * jmax + b_j] + lambda_3[6 * jmax + b_j];
1805 while ((!exitg1) && (b_j < 7)) {
1806 jj = (colj + b_j) - 1;
1808 if (!(b_j - 1 < 1)) {
1811 for (b_iy = 1; b_iy < b_j; b_iy++) {
1812 lambda += SR[ix] * SR[iy];
1825 ix = ((5 - b_j) * 6 + colj) + 7;
1826 for (iy = colj + 7; iy <= ix; iy += 6) {
1830 for (ia = iy; ia <= e; ia++) {
1831 lambda += SR[ia - 1] * SR[b_ix];
1841 ix = ((5 - b_j) * 6 + jj) + 7;
1842 for (i = jj + 6; i + 1 <= ix; i += 6) {
1863 for (b_j = 0; b_j + 1 <= jmax; b_j++) {
1864 for (colj = b_j + 1; colj + 1 <= jmax; colj++) {
1865 SR[colj + 6 * b_j] = 0.0F;
1872 for (jmax = 0; jmax < 6; jmax++) {
1873 for (b_j = 0; b_j < 6; b_j++) {
1874 SR_0[b_j + 6 * jmax] = SR[6 * b_j + jmax];
1878 for (jmax = 0; jmax < 6; jmax++) {
1879 for (b_j = 0; b_j < 6; b_j++) {
1880 SR[b_j + 6 * jmax] = SR_0[6 * jmax + b_j];
1892 memcpy(&Y[0], &rtb_P1[0], 49U *
sizeof(
real32_T));
1901 memset(&d[0], 0, 225U *
sizeof(
real32_T));
1906 for (b_j = 0; b_j < 15; b_j++) {
1907 d[b_j + 15 * b_j] = rtb_Wc[b_j];
1908 for (jmax = 0; jmax < 7; jmax++) {
1909 rtb_X[jmax + 7 * b_j] = rtb_Y[7 * b_j + jmax] - rtb_x1[jmax];
1913 for (jmax = 0; jmax < 7; jmax++) {
1914 for (b_j = 0; b_j < 15; b_j++) {
1915 rtb_Y[jmax + 7 * b_j] = 0.0F;
1916 for (colj = 0; colj < 15; colj++) {
1917 rtb_Y[jmax + 7 * b_j] += rtb_X[7 * colj + jmax] * d[15 * b_j + colj];
1922 for (jmax = 0; jmax < 6; jmax++) {
1923 for (b_j = 0; b_j < 15; b_j++) {
1924 rtb_Z_0[b_j + 15 * jmax] = rtb_Z[6 * b_j + jmax] - rtb_z[jmax];
1928 for (jmax = 0; jmax < 7; jmax++) {
1929 for (b_j = 0; b_j < 6; b_j++) {
1930 K[jmax + 7 * b_j] = 0.0F;
1931 for (colj = 0; colj < 15; colj++) {
1932 K[jmax + 7 * b_j] += rtb_Y[7 * colj + jmax] * rtb_Z_0[15 * b_j + colj];
1937 for (jmax = 0; jmax < 6; jmax++) {
1938 for (b_j = 0; b_j < 6; b_j++) {
1939 SR_0[b_j + 6 * jmax] = SR[6 * b_j + jmax];
1952 for (jmax = 0; jmax < 7; jmax++) {
1953 for (b_j = 0; b_j < 6; b_j++) {
1954 U[jmax + 7 * b_j] = 0.0F;
1955 for (colj = 0; colj < 6; colj++) {
1956 U[jmax + 7 * b_j] +=
K[7 * colj + jmax] * SR[6 * b_j + colj];
1966 tmp[0] =
ukf_U.vk[0];
1967 tmp[1] =
ukf_U.vk[1];
1968 tmp[2] =
ukf_U.vk[2];
1971 tmp[5] =
ukf_U.sideslip;
1972 for (i = 0; i < 6; i++) {
1975 for (jmax = 0; jmax < 7; jmax++) {
1976 for (b_j = 0; b_j < 7; b_j++) {
1977 rtb_P1[jmax + 7 * b_j] = 0.0F;
1978 for (colj = 0; colj < 7; colj++) {
1979 rtb_P1[jmax + 7 * b_j] += Y[7 * jmax + colj] * Y[7 * b_j + colj];
1982 lambda_2[jmax + 7 * b_j] = U[7 * i + jmax] * U[7 * i + b_j];
1986 for (jmax = 0; jmax < 7; jmax++) {
1987 for (b_j = 0; b_j < 7; b_j++) {
1988 SQ[b_j + 7 * jmax] = rtb_P1[7 * jmax + b_j] - lambda_2[7 * jmax + b_j];
1996 while ((!exitg1) && (b_j < 8)) {
1997 jj = (colj + b_j) - 1;
1999 if (!(b_j - 1 < 1)) {
2002 for (b_iy = 1; b_iy < b_j; b_iy++) {
2003 lambda += SQ[ix] * SQ[iy];
2016 ix = ((6 - b_j) * 7 + colj) + 8;
2017 for (iy = colj + 8; iy <= ix; iy += 7) {
2021 for (ia = iy; ia <= e; ia++) {
2022 lambda += SQ[ia - 1] * SQ[b_ix];
2032 ix = ((6 - b_j) * 7 + jj) + 8;
2033 for (b_iy = jj + 7; b_iy + 1 <= ix; b_iy += 7) {
2054 for (b_j = 0; b_j + 1 <= jmax; b_j++) {
2055 for (colj = b_j + 1; colj + 1 <= jmax; colj++) {
2056 SQ[colj + 7 * b_j] = 0.0F;
2060 memcpy(&Y[0], &SQ[0], 49U *
sizeof(
real32_T));
2063 u[i] = tmp[i] - rtb_z[i];
2066 for (i = 0; i < 7; i++) {
2068 for (jmax = 0; jmax < 6; jmax++) {
2069 lambda +=
K[7 * jmax + i] * u[jmax];
2073 for (jmax = 0; jmax < 7; jmax++) {
2074 rtb_P1[jmax + 7 * i] = Y[7 * jmax + i];
2087 for (i = 0; i < 7; i++) {
2088 ukf_DW.Delay1_DSTATE[i] =
p[i];
2094 memcpy(&
ukf_DW.Delay_DSTATE[0], &rtb_P1[0], 49U *
sizeof(
real32_T));