diff options
Diffstat (limited to 'src/modules/attitude_estimator_ekf/codegen/mrdivide.c')
| -rwxr-xr-x | src/modules/attitude_estimator_ekf/codegen/mrdivide.c | 357 |
1 files changed, 357 insertions, 0 deletions
diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c new file mode 100755 index 000000000..a810f22e4 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c @@ -0,0 +1,357 @@ +/*
+ * mrdivide.c
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
+{
+ real32_T b_A[9];
+ int32_T rtemp;
+ int32_T k;
+ 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)fabs(b_A[0]);
+ a21 = (real32_T)fabs(b_A[1]);
+ if (a21 > maxval) {
+ maxval = a21;
+ r1 = 1;
+ r2 = 0;
+ }
+
+ if ((real32_T)fabs(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)fabs(b_A[3 + r3]) > (real32_T)fabs(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];
+ }
+ }
+}
+
+/*
+ *
+ */
+void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
+{
+ real32_T b_A[36];
+ int8_T ipiv[6];
+ int32_T i3;
+ int32_T iy;
+ int32_T j;
+ int32_T c;
+ int32_T ix;
+ real32_T temp;
+ int32_T k;
+ real32_T s;
+ int32_T jy;
+ int32_T ijA;
+ real32_T Y[72];
+ for (i3 = 0; i3 < 6; i3++) {
+ for (iy = 0; iy < 6; iy++) {
+ b_A[iy + 6 * i3] = B[i3 + 6 * iy];
+ }
+
+ ipiv[i3] = (int8_T)(1 + i3);
+ }
+
+ for (j = 0; j < 5; j++) {
+ c = j * 7;
+ iy = 0;
+ ix = c;
+ temp = (real32_T)fabs(b_A[c]);
+ for (k = 2; k <= 6 - j; k++) {
+ ix++;
+ s = (real32_T)fabs(b_A[ix]);
+ if (s > temp) {
+ iy = k - 1;
+ temp = s;
+ }
+ }
+
+ if (b_A[c + iy] != 0.0F) {
+ if (iy != 0) {
+ ipiv[j] = (int8_T)((j + iy) + 1);
+ ix = j;
+ iy += j;
+ for (k = 0; k < 6; k++) {
+ temp = b_A[ix];
+ b_A[ix] = b_A[iy];
+ b_A[iy] = temp;
+ ix += 6;
+ iy += 6;
+ }
+ }
+
+ i3 = (c - j) + 6;
+ for (jy = c + 1; jy + 1 <= i3; jy++) {
+ b_A[jy] /= b_A[c];
+ }
+ }
+
+ iy = c;
+ jy = c + 6;
+ for (k = 1; k <= 5 - j; k++) {
+ temp = b_A[jy];
+ if (b_A[jy] != 0.0F) {
+ ix = c + 1;
+ i3 = (iy - j) + 12;
+ for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
+ b_A[ijA] += b_A[ix] * -temp;
+ ix++;
+ }
+ }
+
+ jy += 6;
+ iy += 6;
+ }
+ }
+
+ for (i3 = 0; i3 < 12; i3++) {
+ for (iy = 0; iy < 6; iy++) {
+ Y[iy + 6 * i3] = A[i3 + 12 * iy];
+ }
+ }
+
+ for (jy = 0; jy < 6; jy++) {
+ if (ipiv[jy] != jy + 1) {
+ for (j = 0; j < 12; j++) {
+ temp = Y[jy + 6 * j];
+ Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
+ Y[(ipiv[jy] + 6 * j) - 1] = temp;
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 6 * j;
+ for (k = 0; k < 6; k++) {
+ iy = 6 * k;
+ if (Y[k + c] != 0.0F) {
+ for (jy = k + 2; jy < 7; jy++) {
+ Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
+ }
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 6 * j;
+ for (k = 5; k > -1; k += -1) {
+ iy = 6 * k;
+ if (Y[k + c] != 0.0F) {
+ Y[k + c] /= b_A[k + iy];
+ for (jy = 0; jy + 1 <= k; jy++) {
+ Y[jy + c] -= Y[k + c] * b_A[jy + iy];
+ }
+ }
+ }
+ }
+
+ for (i3 = 0; i3 < 6; i3++) {
+ for (iy = 0; iy < 12; iy++) {
+ y[iy + 12 * i3] = Y[i3 + 6 * iy];
+ }
+ }
+}
+
+/*
+ *
+ */
+void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
+{
+ real32_T b_A[81];
+ int8_T ipiv[9];
+ int32_T i2;
+ int32_T iy;
+ int32_T j;
+ int32_T c;
+ int32_T ix;
+ real32_T temp;
+ int32_T k;
+ real32_T s;
+ int32_T jy;
+ int32_T ijA;
+ real32_T Y[108];
+ for (i2 = 0; i2 < 9; i2++) {
+ for (iy = 0; iy < 9; iy++) {
+ b_A[iy + 9 * i2] = B[i2 + 9 * iy];
+ }
+
+ ipiv[i2] = (int8_T)(1 + i2);
+ }
+
+ for (j = 0; j < 8; j++) {
+ c = j * 10;
+ iy = 0;
+ ix = c;
+ temp = (real32_T)fabs(b_A[c]);
+ for (k = 2; k <= 9 - j; k++) {
+ ix++;
+ s = (real32_T)fabs(b_A[ix]);
+ if (s > temp) {
+ iy = k - 1;
+ temp = s;
+ }
+ }
+
+ if (b_A[c + iy] != 0.0F) {
+ if (iy != 0) {
+ ipiv[j] = (int8_T)((j + iy) + 1);
+ ix = j;
+ iy += j;
+ for (k = 0; k < 9; k++) {
+ temp = b_A[ix];
+ b_A[ix] = b_A[iy];
+ b_A[iy] = temp;
+ ix += 9;
+ iy += 9;
+ }
+ }
+
+ i2 = (c - j) + 9;
+ for (jy = c + 1; jy + 1 <= i2; jy++) {
+ b_A[jy] /= b_A[c];
+ }
+ }
+
+ iy = c;
+ jy = c + 9;
+ for (k = 1; k <= 8 - j; k++) {
+ temp = b_A[jy];
+ if (b_A[jy] != 0.0F) {
+ ix = c + 1;
+ i2 = (iy - j) + 18;
+ for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
+ b_A[ijA] += b_A[ix] * -temp;
+ ix++;
+ }
+ }
+
+ jy += 9;
+ iy += 9;
+ }
+ }
+
+ for (i2 = 0; i2 < 12; i2++) {
+ for (iy = 0; iy < 9; iy++) {
+ Y[iy + 9 * i2] = A[i2 + 12 * iy];
+ }
+ }
+
+ for (jy = 0; jy < 9; jy++) {
+ if (ipiv[jy] != jy + 1) {
+ for (j = 0; j < 12; j++) {
+ temp = Y[jy + 9 * j];
+ Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
+ Y[(ipiv[jy] + 9 * j) - 1] = temp;
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 9 * j;
+ for (k = 0; k < 9; k++) {
+ iy = 9 * k;
+ if (Y[k + c] != 0.0F) {
+ for (jy = k + 2; jy < 10; jy++) {
+ Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
+ }
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 9 * j;
+ for (k = 8; k > -1; k += -1) {
+ iy = 9 * k;
+ if (Y[k + c] != 0.0F) {
+ Y[k + c] /= b_A[k + iy];
+ for (jy = 0; jy + 1 <= k; jy++) {
+ Y[jy + c] -= Y[k + c] * b_A[jy + iy];
+ }
+ }
+ }
+ }
+
+ for (i2 = 0; i2 < 9; i2++) {
+ for (iy = 0; iy < 12; iy++) {
+ y[iy + 12 * i2] = Y[i2 + 9 * iy];
+ }
+ }
+}
+
+/* End of code generation (mrdivide.c) */
|