diff --git a/Estimators/QEKF/codegen/lib/QEKF/QEKF.cpp b/Estimators/QEKF/codegen/lib/QEKF/QEKF.cpp new file mode 100644 index 0000000..33555cc --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/QEKF.cpp @@ -0,0 +1,1545 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: QEKF.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// + +// Include Files +#include +#include "rt_defines.h" +#include "rt_nonfinite.h" +#include +#include "QEKF.h" +#include "norm.h" +#include "eye.h" +#include "mrdivide.h" + +// Variable Definitions +static float acc_norm_filtered; +static boolean_T acc_norm_filtered_not_empty; +static float acc_norm_old; +static boolean_T acc_norm_old_not_empty; + +// Function Declarations +static float rt_atan2f_snf(float u0, float u1); + +// Function Definitions + +// +// Arguments : float u0 +// float u1 +// Return Type : float +// +static float rt_atan2f_snf(float u0, float u1) +{ + float y; + int b_u0; + int b_u1; + if (rtIsNaNF(u0) || rtIsNaNF(u1)) { + y = ((real32_T)rtNaN); + } else if (rtIsInfF(u0) && rtIsInfF(u1)) { + if (u0 > 0.0F) { + b_u0 = 1; + } else { + b_u0 = -1; + } + + if (u1 > 0.0F) { + b_u1 = 1; + } else { + b_u1 = -1; + } + + y = (float)atan2((double)(float)b_u0, (double)(float)b_u1); + } 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 = (float)atan2((double)u0, (double)u1); + } + + return y; +} + +// +// function [X_out, P_out] = QEKF(X, P_prev, Gyroscope, Accelerometer, Heading, UseHeadingForCorrection, SamplePeriod, SensorDriven, BiasEstimationEnabled, YawBiasEstimationEnabled, NormalizeAccelerometer, cov_gyro, cov_acc, GyroscopeTrustFactor, sigma2_omega, sigma2_heading, sigma2_bias, AccelerometerVibrationDetectionEnabled, AccelerometerVibrationNormLPFtau, AccelerometerVibrationCovarianceVaryFactor, MaxVaryFactor, g) +// for q o p = Phi(q) * p +// Arguments : const float X[10] +// const float P_prev[100] +// const float Gyroscope[3] +// const float Accelerometer[3] +// float Heading +// boolean_T UseHeadingForCorrection +// float SamplePeriod +// boolean_T SensorDriven +// boolean_T BiasEstimationEnabled +// boolean_T YawBiasEstimationEnabled +// boolean_T NormalizeAccelerometer +// const float cov_gyro[9] +// const float cov_acc[9] +// float GyroscopeTrustFactor +// float sigma2_omega +// float sigma2_heading +// float sigma2_bias +// boolean_T AccelerometerVibrationDetectionEnabled +// float AccelerometerVibrationNormLPFtau +// float AccelerometerVibrationCovarianceVaryFactor +// float MaxVaryFactor +// float g +// float X_out[10] +// float P_out[100] +// Return Type : void +// +void QEKF(const float X[10], const float P_prev[100], const float Gyroscope[3], + const float Accelerometer[3], float Heading, boolean_T + UseHeadingForCorrection, float SamplePeriod, boolean_T SensorDriven, + boolean_T BiasEstimationEnabled, boolean_T YawBiasEstimationEnabled, + boolean_T NormalizeAccelerometer, const float cov_gyro[9], const float + cov_acc[9], float GyroscopeTrustFactor, float sigma2_omega, float + sigma2_heading, float sigma2_bias, boolean_T + AccelerometerVibrationDetectionEnabled, float + AccelerometerVibrationNormLPFtau, float + AccelerometerVibrationCovarianceVaryFactor, float MaxVaryFactor, float + g, float X_out[10], float P_out[100]) +{ + float q[4]; + float gyro_bias[3]; + float gyro_input[3]; + int i0; + signed char BiasEstimationEnabledMat[9]; + float cov_omega[9]; + static const signed char iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + float a; + float coeff_b; + int i; + static float Q[100]; + float z_acc[3]; + float R[36]; + float z[6]; + float X_apriori[10]; + float fv0[16]; + float b_X[4]; + float dq[12]; + int i1; + float b_dq[4]; + static const signed char iv1[12] = { 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + static float F_prev[100]; + double dv0[16]; + float y[16]; + float b_y[4]; + double dv1[9]; + static float b_P_prev[100]; + float c_y[16]; + static float P_apriori[100]; + float d_y[16]; + float c_dq[12]; + static const signed char iv2[12] = { 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1 }; + + static const signed char iv3[4] = { 0, 0, 0, -1 }; + + float b_cov_gyro[12]; + float fv1[4]; + float fv2[4]; + float fv3[4]; + static const signed char iv4[16] = { 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, + 0, 0, -1 }; + + float H_acc[30]; + float z_hat[6]; + static float H[60]; + static float b_H[60]; + float c_H[36]; + static float K[60]; + float b_z[6]; + static double dv2[100]; + float c_z[10]; + float d_dq[8]; + float heading_vector_hat[2]; + float e_dq[8]; + static const signed char iv5[8] = { 0, 1, 0, 0, 0, 0, 1, 0 }; + + float b_heading_vector_hat[2]; + static const signed char iv6[4] = { 0, 1, 0, 0 }; + + float fv4[20]; + static float H2[70]; + static float b_H2[70]; + static float K2[70]; + static float b_R[49]; + static float c_H2[49]; + static float d_H2[49]; + float d_z[7]; + float b_z_hat[7]; + float e_z[7]; + + // 'QEKF:3' Phi = @(q)[q(1) -q(2) -q(3) -q(4); % for q o p = Phi(q) * p + // 'QEKF:4' q(2) q(1) -q(4) q(3); + // 'QEKF:5' q(3) q(4) q(1) -q(2); + // 'QEKF:6' q(4) -q(3) q(2) q(1)]; + // for q o p = Gamma(p) * q + // 'QEKF:7' Gamma = @(p)[p(1) -p(2) -p(3) -p(4); % for q o p = Gamma(p) * q + // 'QEKF:8' p(2) p(1) p(4) -p(3); + // 'QEKF:9' p(3) -p(4) p(1) p(2); + // 'QEKF:10' p(4) p(3) -p(2) p(1)]; + // 'QEKF:12' devec = [0,1,0,0;0,0,1,0;0,0,0,1]; + // 'v' in notes + // 'QEKF:13' vec = [0,0,0;1,0,0;0,1,0;0,0,1]; + // '^' in notes + // 'QEKF:14' I_conj = diag([1,-1,-1,-1]); + // 'QEKF:16' dt = SamplePeriod; + // Split state vector, X[k-1], into individual variables + // 'QEKF:19' q1 = X(1); + // 'QEKF:20' q2 = X(2); + // 'QEKF:21' q3 = X(3); + // 'QEKF:22' q4 = X(4); + // 'QEKF:23' q = [q1, q2, q3, q4]'; + q[0] = X[0]; + q[1] = X[1]; + q[2] = X[2]; + q[3] = X[3]; + + // 'QEKF:25' omega = X(5:7); + // 'QEKF:27' gyro_bias_x = X(8); + // 'QEKF:28' gyro_bias_y = X(9); + // 'QEKF:29' gyro_bias_z = X(10); + // 'QEKF:30' gyro_bias = [gyro_bias_x; gyro_bias_y; gyro_bias_z]; + gyro_bias[0] = X[7]; + gyro_bias[1] = X[8]; + gyro_bias[2] = X[9]; + + // 'QEKF:32' gyro_x = Gyroscope(1); + // 'QEKF:33' gyro_y = Gyroscope(2); + // 'QEKF:34' gyro_z = Gyroscope(3); + // 'QEKF:35' gyro_input = [gyro_x; gyro_y; gyro_z]; + gyro_input[0] = Gyroscope[0]; + gyro_input[1] = Gyroscope[1]; + gyro_input[2] = Gyroscope[2]; + + // 'QEKF:37' BiasEstimationEnabledMat = single(zeros(3,3)); + // 'QEKF:38' BiasEstimationEnabledMat(1,1) = BiasEstimationEnabled; + for (i0 = 0; i0 < 9; i0++) { + BiasEstimationEnabledMat[i0] = 0; + cov_omega[i0] = sigma2_omega * (float)iv0[i0]; + } + + BiasEstimationEnabledMat[0] = (signed char)BiasEstimationEnabled; + + // 'QEKF:39' BiasEstimationEnabledMat(2,2) = BiasEstimationEnabled; + BiasEstimationEnabledMat[4] = (signed char)BiasEstimationEnabled; + + // 'QEKF:40' BiasEstimationEnabledMat(3,3) = BiasEstimationEnabled*YawBiasEstimationEnabled; + BiasEstimationEnabledMat[8] = (signed char)(BiasEstimationEnabled * + YawBiasEstimationEnabled); + + // Process covariances + // 'QEKF:43' cov_q = single(zeros(4,4)); + // quaternion kinematics is correct since we propagate with Quaternion exponential + // 'QEKF:44' cov_omega = sigma2_omega * eye(3); + // 'QEKF:45' cov_bias = sigma2_bias*dt*BiasEstimationEnabledMat + zeros(3,3); + a = sigma2_bias * SamplePeriod; + + // bias stays constant + // Vary gyroscope covariance based on accelerometer vibrations noise - + // trust gyroscope more at high accelerometer vibrations + // 'QEKF:50' if (isempty(acc_norm_filtered)) + if (!acc_norm_filtered_not_empty) { + // 'QEKF:51' acc_norm_filtered = g; + acc_norm_filtered = g; + acc_norm_filtered_not_empty = true; + } + + // 'QEKF:55' if (isempty(acc_norm_old)) + if (!acc_norm_old_not_empty) { + // 'QEKF:56' acc_norm_old = g; + acc_norm_old = g; + acc_norm_old_not_empty = true; + } + + // 'QEKF:59' if (AccelerometerVibrationDetectionEnabled) + if (AccelerometerVibrationDetectionEnabled) { + // 'QEKF:60' coeff_b = 1/(2*AccelerometerVibrationNormLPFtau/dt + 1); + coeff_b = 1.0F / (2.0F * AccelerometerVibrationNormLPFtau / SamplePeriod + + 1.0F); + + // nominator + // 'QEKF:61' coeff_a = 1/(2*AccelerometerVibrationNormLPFtau/dt + 1) - 2/(2 + dt/AccelerometerVibrationNormLPFtau); + // denominator + // 'QEKF:62' acc_norm_filtered = coeff_b*norm(Accelerometer) + coeff_b*acc_norm_old - coeff_a*acc_norm_filtered; + acc_norm_filtered = (coeff_b * norm(Accelerometer) + coeff_b * acc_norm_old) + - (1.0F / (2.0F * AccelerometerVibrationNormLPFtau / SamplePeriod + 1.0F) + - 2.0F / (2.0F + SamplePeriod / AccelerometerVibrationNormLPFtau)) * + acc_norm_filtered; + + // 'QEKF:63' acc_norm_old = norm(Accelerometer); + acc_norm_old = norm(Accelerometer); + + // if (abs(acc_norm_filtered - g) > AccelerometerExaggerationAmount) + // 'QEKF:66' VaryFactor = exp(AccelerometerVibrationCovarianceVaryFactor * (abs(acc_norm_filtered - g))); + coeff_b = (float)exp((double)(AccelerometerVibrationCovarianceVaryFactor * + (float)fabs((double)(acc_norm_filtered - g)))); + + // 'QEKF:67' VaryFactor = min(VaryFactor, MaxVaryFactor); + if (!((coeff_b < MaxVaryFactor) || rtIsNaNF(MaxVaryFactor))) { + coeff_b = MaxVaryFactor; + } + + // cov_acc_ = cov_acc_ * VaryFactor; + // 'QEKF:69' cov_omega = cov_omega / VaryFactor; + for (i0 = 0; i0 < 9; i0++) { + cov_omega[i0] /= coeff_b; + } + + // end + // 'QEKF:71' acc_norm_out = acc_norm_filtered; + } + + // Setup covariance matrices + // 'QEKF:75' Q = [cov_q, zeros(4,3), zeros(4,3); + // 'QEKF:76' zeros(3,4), cov_omega, zeros(3,3); + // 'QEKF:77' zeros(3,4), zeros(3,3), cov_bias]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 4; i++) { + Q[i + 10 * i0] = 0.0F; + } + + for (i = 0; i < 3; i++) { + Q[(i + 10 * i0) + 4] = 0.0F; + Q[(i + 10 * i0) + 7] = 0.0F; + } + } + + // 'QEKF:79' R = [GyroscopeTrustFactor*cov_acc, zeros(3,3) + // 'QEKF:80' zeros(3,3), cov_gyro]; + for (i0 = 0; i0 < 3; i0++) { + for (i = 0; i < 4; i++) { + Q[i + 10 * (i0 + 4)] = 0.0F; + } + + for (i = 0; i < 3; i++) { + Q[(i + 10 * (i0 + 4)) + 4] = cov_omega[i + 3 * i0]; + Q[(i + 10 * (i0 + 4)) + 7] = 0.0F; + } + + for (i = 0; i < 4; i++) { + Q[i + 10 * (i0 + 7)] = 0.0F; + } + + for (i = 0; i < 3; i++) { + Q[(i + 10 * (i0 + 7)) + 4] = 0.0F; + Q[(i + 10 * (i0 + 7)) + 7] = a * (float)BiasEstimationEnabledMat[i + 3 * + i0]; + R[i + 6 * i0] = GyroscopeTrustFactor * cov_acc[i + 3 * i0]; + R[(i + 6 * i0) + 3] = 0.0F; + R[i + 6 * (i0 + 3)] = 0.0F; + R[(i + 6 * (i0 + 3)) + 3] = cov_gyro[i + 3 * i0]; + } + } + + // Measurement vector + // Normalize accelerometer seems to give better results + // 'QEKF:84' if (NormalizeAccelerometer) + if (NormalizeAccelerometer) { + // norm_acc = norm(Accelerometer); + // if (norm_acc > 0)% && norm_acc < 9.85) + // z_acc = Accelerometer / norm_acc; + // else + // z_acc = 0*Accelerometer; + // end + // 'QEKF:91' z_acc = Accelerometer / g; + for (i = 0; i < 3; i++) { + z_acc[i] = Accelerometer[i] / g; + } + + // this is not actually a normalization, but rather a nominal normalization - but it seems to yield better results? + } else { + // 'QEKF:92' else + // 'QEKF:93' z_acc = Accelerometer; + for (i = 0; i < 3; i++) { + z_acc[i] = Accelerometer[i]; + } + } + + // 'QEKF:96' z_gyro = gyro_input; + // 'QEKF:98' z = [z_acc; z_gyro]; + for (i = 0; i < 3; i++) { + z[i] = z_acc[i]; + z[i + 3] = gyro_input[i]; + } + + // %% Prediction step + // 'QEKF:101' X_apriori = single(zeros(10,1)); + for (i = 0; i < 10; i++) { + X_apriori[i] = 0.0F; + } + + // Propagate quaternion + // 'QEKF:104' if (SensorDriven) + if (SensorDriven) { + // 'QEKF:105' dq = 1/2 * Phi(q) * vec * (gyro_input - gyro_bias); + fv0[0] = 0.5F * X[0]; + fv0[1] = 0.5F * -X[1]; + fv0[2] = 0.5F * -X[2]; + fv0[3] = 0.5F * -X[3]; + fv0[4] = 0.5F * X[1]; + fv0[5] = 0.5F * X[0]; + fv0[6] = 0.5F * -X[3]; + fv0[7] = 0.5F * X[2]; + fv0[8] = 0.5F * X[2]; + fv0[9] = 0.5F * X[3]; + fv0[10] = 0.5F * X[0]; + fv0[11] = 0.5F * -X[1]; + fv0[12] = 0.5F * X[3]; + fv0[13] = 0.5F * -X[2]; + fv0[14] = 0.5F * X[1]; + fv0[15] = 0.5F * X[0]; + for (i0 = 0; i0 < 3; i0++) { + z_acc[i0] = gyro_input[i0] - gyro_bias[i0]; + for (i = 0; i < 4; i++) { + dq[i0 + 3 * i] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + dq[i0 + 3 * i] += (float)iv1[i0 + 3 * i1] * fv0[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + b_X[i0] = 0.0F; + for (i = 0; i < 3; i++) { + b_X[i0] += z_acc[i] * dq[i + 3 * i0]; + } + + b_dq[i0] = b_X[i0]; + } + } else { + // 'QEKF:106' else + // 'QEKF:107' dq = 1/2 * Phi(q) * vec * omega; + fv0[0] = 0.5F * X[0]; + fv0[1] = 0.5F * -X[1]; + fv0[2] = 0.5F * -X[2]; + fv0[3] = 0.5F * -X[3]; + fv0[4] = 0.5F * X[1]; + fv0[5] = 0.5F * X[0]; + fv0[6] = 0.5F * -X[3]; + fv0[7] = 0.5F * X[2]; + fv0[8] = 0.5F * X[2]; + fv0[9] = 0.5F * X[3]; + fv0[10] = 0.5F * X[0]; + fv0[11] = 0.5F * -X[1]; + fv0[12] = 0.5F * X[3]; + fv0[13] = 0.5F * -X[2]; + fv0[14] = 0.5F * X[1]; + fv0[15] = 0.5F * X[0]; + for (i0 = 0; i0 < 3; i0++) { + for (i = 0; i < 4; i++) { + dq[i0 + 3 * i] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + dq[i0 + 3 * i] += (float)iv1[i0 + 3 * i1] * fv0[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + b_X[i0] = 0.0F; + for (i = 0; i < 3; i++) { + b_X[i0] += X[4 + i] * dq[i + 3 * i0]; + } + + b_dq[i0] = b_X[i0]; + } + + // and conversely: omeg = 2 * devec * Phi(q)' * dq; + } + + // 'QEKF:109' q_apriori = q + dt * dq; + for (i0 = 0; i0 < 4; i0++) { + b_dq[i0] = q[i0] + SamplePeriod * b_dq[i0]; + } + + // Forward Euler + // q_apriori = Phi(q) * [1; 1/2*dt*omega]; % Delta-Quaternion integration + // Propagate/set angular velocity states + // 'QEKF:113' omega_apriori = omega; + // Propagate gyro bias + // 'QEKF:116' gyro_bias_apriori = gyro_bias; + // Determine model Jacobian (F) - OBS. This is not supposed to use/depend on apriori states! + // 'QEKF:119' F_prev = single(zeros(10,10)); + memset(&F_prev[0], 0, 100U * sizeof(float)); + + // 'QEKF:120' F_prev(1:4,1:4) = eye(4) + dt * 1/2 * Gamma(vec*omega); + coeff_b = SamplePeriod / 2.0F; + for (i0 = 0; i0 < 4; i0++) { + b_X[i0] = 0.0F; + for (i = 0; i < 3; i++) { + b_X[i0] += X[4 + i] * (float)iv1[i + 3 * i0]; + } + + b_y[i0] = b_X[i0]; + } + + b_eye(dv0); + y[0] = coeff_b * b_y[0]; + y[1] = coeff_b * -b_y[1]; + y[2] = coeff_b * -b_y[2]; + y[3] = coeff_b * -b_y[3]; + y[4] = coeff_b * b_y[1]; + y[5] = coeff_b * b_y[0]; + y[6] = coeff_b * b_y[3]; + y[7] = coeff_b * -b_y[2]; + y[8] = coeff_b * b_y[2]; + y[9] = coeff_b * -b_y[3]; + y[10] = coeff_b * b_y[0]; + y[11] = coeff_b * b_y[1]; + y[12] = coeff_b * b_y[3]; + y[13] = coeff_b * b_y[2]; + y[14] = coeff_b * -b_y[1]; + y[15] = coeff_b * b_y[0]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 4; i++) { + F_prev[i + 10 * i0] = (float)dv0[i + (i0 << 2)] + y[i + (i0 << 2)]; + } + } + + // Gamma([1, 1/2*dt*omega]); % eye(4); + // 'QEKF:121' F_prev(1:4,5:7) = dt * 1/2 * Phi(q) * [zeros(1,3); eye(3)]; + coeff_b = SamplePeriod / 2.0F; + y[0] = coeff_b * X[0]; + y[1] = coeff_b * -X[1]; + y[2] = coeff_b * -X[2]; + y[3] = coeff_b * -X[3]; + y[4] = coeff_b * X[1]; + y[5] = coeff_b * X[0]; + y[6] = coeff_b * -X[3]; + y[7] = coeff_b * X[2]; + y[8] = coeff_b * X[2]; + y[9] = coeff_b * X[3]; + y[10] = coeff_b * X[0]; + y[11] = coeff_b * -X[1]; + y[12] = coeff_b * X[3]; + y[13] = coeff_b * -X[2]; + y[14] = coeff_b * X[1]; + y[15] = coeff_b * X[0]; + for (i0 = 0; i0 < 3; i0++) { + for (i = 0; i < 4; i++) { + F_prev[(i0 + 10 * i) + 4] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + F_prev[(i0 + 10 * i) + 4] += (float)iv1[i0 + 3 * i1] * y[i1 + (i << 2)]; + } + } + } + + // 'QEKF:122' F_prev(1:4,8:10) = zeros(4,3); + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 3; i++) { + F_prev[(i + 10 * i0) + 7] = 0.0F; + } + } + + // 'QEKF:123' F_prev(5:7,1:4) = zeros(3,4); + // 'QEKF:124' F_prev(5:7,5:7) = eye(3); + eye(dv1); + + // 'QEKF:125' F_prev(5:7,8:10) = zeros(3,3); + // 'QEKF:126' F_prev(8:10,1:4) = zeros(3,4); + // 'QEKF:127' F_prev(8:10,5:7) = zeros(3,3); + for (i0 = 0; i0 < 3; i0++) { + for (i = 0; i < 4; i++) { + F_prev[i + 10 * (4 + i0)] = 0.0F; + } + + for (i = 0; i < 3; i++) { + F_prev[(i + 10 * (4 + i0)) + 4] = (float)dv1[i + 3 * i0]; + F_prev[(i + 10 * (4 + i0)) + 7] = 0.0F; + } + + for (i = 0; i < 4; i++) { + F_prev[i + 10 * (7 + i0)] = 0.0F; + } + + for (i = 0; i < 3; i++) { + F_prev[(i + 10 * (7 + i0)) + 4] = 0.0F; + } + } + + // 'QEKF:128' F_prev(8:10,8:10) = eye(3); + eye(dv1); + for (i0 = 0; i0 < 3; i0++) { + for (i = 0; i < 3; i++) { + F_prev[(i + 10 * (7 + i0)) + 7] = (float)dv1[i + 3 * i0]; + } + } + + // 'QEKF:130' if (SensorDriven) + if (SensorDriven) { + // Change covariance matrix and model Jacobian + // 'QEKF:132' F_prev(1:4,1:4) = eye(4); + b_eye(dv0); + + // 'QEKF:133' F_prev(1:4,5:7) = zeros(4,3); + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 4; i++) { + F_prev[i + 10 * i0] = (float)dv0[i + (i0 << 2)]; + } + + for (i = 0; i < 3; i++) { + F_prev[(i + 10 * i0) + 4] = 0.0F; + } + } + + // 'QEKF:134' F_prev(1:4,8:10) = -dt * 1/2 * Phi(q) * [zeros(1,3); BiasEstimationEnabledMat]; + coeff_b = -SamplePeriod / 2.0F; + for (i0 = 0; i0 < 3; i0++) { + dq[i0] = 0.0F; + for (i = 0; i < 3; i++) { + dq[i + 3 * (i0 + 1)] = BiasEstimationEnabledMat[i + 3 * i0]; + } + } + + y[0] = coeff_b * X[0]; + y[1] = coeff_b * -X[1]; + y[2] = coeff_b * -X[2]; + y[3] = coeff_b * -X[3]; + y[4] = coeff_b * X[1]; + y[5] = coeff_b * X[0]; + y[6] = coeff_b * -X[3]; + y[7] = coeff_b * X[2]; + y[8] = coeff_b * X[2]; + y[9] = coeff_b * X[3]; + y[10] = coeff_b * X[0]; + y[11] = coeff_b * -X[1]; + y[12] = coeff_b * X[3]; + y[13] = coeff_b * -X[2]; + y[14] = coeff_b * X[1]; + y[15] = coeff_b * X[0]; + + // 'QEKF:135' Q(1:4,1:4) = (dt * 1/2 * Phi(q) * [zeros(1,3); eye(3)]) * cov_gyro * (dt * 1/2 * Phi(q) * [zeros(1,3); eye(3)])'; + coeff_b = SamplePeriod / 2.0F; + a = SamplePeriod / 2.0F; + c_y[0] = a * X[0]; + c_y[1] = a * -X[1]; + c_y[2] = a * -X[2]; + c_y[3] = a * -X[3]; + c_y[4] = a * X[1]; + c_y[5] = a * X[0]; + c_y[6] = a * -X[3]; + c_y[7] = a * X[2]; + c_y[8] = a * X[2]; + c_y[9] = a * X[3]; + c_y[10] = a * X[0]; + c_y[11] = a * -X[1]; + c_y[12] = a * X[3]; + c_y[13] = a * -X[2]; + c_y[14] = a * X[1]; + c_y[15] = a * X[0]; + d_y[0] = coeff_b * X[0]; + d_y[1] = coeff_b * -X[1]; + d_y[2] = coeff_b * -X[2]; + d_y[3] = coeff_b * -X[3]; + d_y[4] = coeff_b * X[1]; + d_y[5] = coeff_b * X[0]; + d_y[6] = coeff_b * -X[3]; + d_y[7] = coeff_b * X[2]; + d_y[8] = coeff_b * X[2]; + d_y[9] = coeff_b * X[3]; + d_y[10] = coeff_b * X[0]; + d_y[11] = coeff_b * -X[1]; + d_y[12] = coeff_b * X[3]; + d_y[13] = coeff_b * -X[2]; + d_y[14] = coeff_b * X[1]; + d_y[15] = coeff_b * X[0]; + for (i0 = 0; i0 < 3; i0++) { + for (i = 0; i < 4; i++) { + F_prev[(i0 + 10 * i) + 7] = 0.0F; + c_dq[i0 + 3 * i] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + F_prev[(i0 + 10 * i) + 7] += dq[i0 + 3 * i1] * y[i1 + (i << 2)]; + c_dq[i0 + 3 * i] += (float)iv1[i0 + 3 * i1] * d_y[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 3; i++) { + dq[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + dq[i0 + (i << 2)] += (float)iv1[i + 3 * i1] * c_y[i1 + (i0 << 2)]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + for (i = 0; i < 4; i++) { + b_cov_gyro[i0 + 3 * i] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + b_cov_gyro[i0 + 3 * i] += cov_gyro[i0 + 3 * i1] * c_dq[i1 + 3 * i]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 4; i++) { + Q[i0 + 10 * i] = 0.0F; + for (i1 = 0; i1 < 3; i1++) { + Q[i0 + 10 * i] += dq[i0 + (i1 << 2)] * b_cov_gyro[i1 + 3 * i]; + } + } + } + } + + // Set apriori state + // 'QEKF:139' X_apriori(1:4) = q_apriori; + for (i = 0; i < 4; i++) { + X_apriori[i] = b_dq[i]; + } + + // 'QEKF:140' X_apriori(5:7) = omega_apriori; + // 'QEKF:141' X_apriori(8:10) = gyro_bias_apriori; + for (i = 0; i < 3; i++) { + X_apriori[i + 4] = X[i + 4]; + X_apriori[i + 7] = gyro_bias[i]; + } + + // Calculate apriori covariance of estimate error + // 'QEKF:144' P_apriori = F_prev * P_prev * F_prev' + Q; + for (i0 = 0; i0 < 10; i0++) { + for (i = 0; i < 10; i++) { + b_P_prev[i0 + 10 * i] = 0.0F; + for (i1 = 0; i1 < 10; i1++) { + b_P_prev[i0 + 10 * i] += P_prev[i0 + 10 * i1] * F_prev[i1 + 10 * i]; + } + } + } + + for (i0 = 0; i0 < 10; i0++) { + for (i = 0; i < 10; i++) { + coeff_b = 0.0F; + for (i1 = 0; i1 < 10; i1++) { + coeff_b += F_prev[i1 + 10 * i0] * b_P_prev[i1 + 10 * i]; + } + + P_apriori[i0 + 10 * i] = coeff_b + Q[i0 + 10 * i]; + } + } + + // %% Update/correction step + // 'QEKF:147' if (NormalizeAccelerometer) + if (NormalizeAccelerometer) { + // Accelerometer Measurement model + // 'QEKF:149' z_hat_acc = -devec * Phi(q_apriori)' * Gamma(q_apriori) * [0;0;0;-1]; + c_y[0] = b_dq[0]; + c_y[4] = -b_dq[1]; + c_y[8] = -b_dq[2]; + c_y[12] = -b_dq[3]; + c_y[1] = b_dq[1]; + c_y[5] = b_dq[0]; + c_y[9] = -b_dq[3]; + c_y[13] = b_dq[2]; + c_y[2] = b_dq[2]; + c_y[6] = b_dq[3]; + c_y[10] = b_dq[0]; + c_y[14] = -b_dq[1]; + c_y[3] = b_dq[3]; + c_y[7] = -b_dq[2]; + c_y[11] = b_dq[1]; + c_y[15] = b_dq[0]; + d_y[0] = b_dq[0]; + d_y[1] = -b_dq[1]; + d_y[2] = -b_dq[2]; + d_y[3] = -b_dq[3]; + d_y[4] = b_dq[1]; + d_y[5] = b_dq[0]; + d_y[6] = b_dq[3]; + d_y[7] = -b_dq[2]; + d_y[8] = b_dq[2]; + d_y[9] = -b_dq[3]; + d_y[10] = b_dq[0]; + d_y[11] = b_dq[1]; + d_y[12] = b_dq[3]; + d_y[13] = b_dq[2]; + d_y[14] = -b_dq[1]; + d_y[15] = b_dq[0]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 3; i++) { + dq[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + dq[i0 + (i << 2)] += c_y[i0 + (i1 << 2)] * (float)iv2[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 3; i++) { + c_dq[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + c_dq[i0 + (i << 2)] += d_y[i0 + (i1 << 2)] * dq[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + z_acc[i0] = 0.0F; + for (i = 0; i < 4; i++) { + z_acc[i0] += (float)iv3[i] * c_dq[i + (i0 << 2)]; + } + + gyro_input[i0] = z_acc[i0]; + } + + // Measurement Jacobian + // 'QEKF:152' H_acc = [-devec * (Gamma(Gamma(q_apriori)*[0;0;0;-1])*I_conj + Phi(Phi(q_apriori)'*[0;0;0;-1])), ... + // 'QEKF:153' zeros(3,3), ... + // 'QEKF:154' zeros(3,3)]; + c_y[0] = b_dq[0]; + c_y[1] = -b_dq[1]; + c_y[2] = -b_dq[2]; + c_y[3] = -b_dq[3]; + c_y[4] = b_dq[1]; + c_y[5] = b_dq[0]; + c_y[6] = b_dq[3]; + c_y[7] = -b_dq[2]; + c_y[8] = b_dq[2]; + c_y[9] = -b_dq[3]; + c_y[10] = b_dq[0]; + c_y[11] = b_dq[1]; + c_y[12] = b_dq[3]; + c_y[13] = b_dq[2]; + c_y[14] = -b_dq[1]; + c_y[15] = b_dq[0]; + d_y[0] = b_dq[0]; + d_y[4] = -b_dq[1]; + d_y[8] = -b_dq[2]; + d_y[12] = -b_dq[3]; + d_y[1] = b_dq[1]; + d_y[5] = b_dq[0]; + d_y[9] = -b_dq[3]; + d_y[13] = b_dq[2]; + d_y[2] = b_dq[2]; + d_y[6] = b_dq[3]; + d_y[10] = b_dq[0]; + d_y[14] = -b_dq[1]; + d_y[3] = b_dq[3]; + d_y[7] = -b_dq[2]; + d_y[11] = b_dq[1]; + d_y[15] = b_dq[0]; + for (i0 = 0; i0 < 4; i0++) { + b_X[i0] = 0.0F; + for (i = 0; i < 4; i++) { + b_X[i0] += (float)iv3[i] * c_y[i + (i0 << 2)]; + } + + b_y[i0] = b_X[i0]; + fv2[i0] = 0.0F; + for (i = 0; i < 4; i++) { + fv2[i0] += (float)iv3[i] * d_y[i + (i0 << 2)]; + } + + q[i0] = fv2[i0]; + } + + y[0] = b_y[0]; + y[1] = -b_y[1]; + y[2] = -b_y[2]; + y[3] = -b_y[3]; + y[4] = b_y[1]; + y[5] = b_y[0]; + y[6] = b_y[3]; + y[7] = -b_y[2]; + y[8] = b_y[2]; + y[9] = -b_y[3]; + y[10] = b_y[0]; + y[11] = b_y[1]; + y[12] = b_y[3]; + y[13] = b_y[2]; + y[14] = -b_y[1]; + y[15] = b_y[0]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 4; i++) { + fv0[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + fv0[i0 + (i << 2)] += (float)iv4[i0 + (i1 << 2)] * y[i1 + (i << 2)]; + } + } + } + + c_y[0] = q[0]; + c_y[1] = -q[1]; + c_y[2] = -q[2]; + c_y[3] = -q[3]; + c_y[4] = q[1]; + c_y[5] = q[0]; + c_y[6] = -q[3]; + c_y[7] = q[2]; + c_y[8] = q[2]; + c_y[9] = q[3]; + c_y[10] = q[0]; + c_y[11] = -q[1]; + c_y[12] = q[3]; + c_y[13] = -q[2]; + c_y[14] = q[1]; + c_y[15] = q[0]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 4; i++) { + d_y[i + (i0 << 2)] = fv0[i + (i0 << 2)] + c_y[i + (i0 << 2)]; + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 3; i++) { + dq[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + dq[i0 + (i << 2)] += d_y[i0 + (i1 << 2)] * (float)iv2[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + for (i = 0; i < 4; i++) { + H_acc[i + 10 * i0] = dq[i + (i0 << 2)]; + } + + for (i = 0; i < 3; i++) { + H_acc[(i + 10 * i0) + 4] = 0.0F; + H_acc[(i + 10 * i0) + 7] = 0.0F; + } + } + } else { + // 'QEKF:155' else + // Accelerometer Measurement model + // 'QEKF:157' z_hat_acc = -devec * Phi(q_apriori)' * Gamma(q_apriori) * [0;0;0;-g]; + b_X[0] = 0.0F; + b_X[1] = 0.0F; + b_X[2] = 0.0F; + b_X[3] = -g; + c_y[0] = b_dq[0]; + c_y[4] = -b_dq[1]; + c_y[8] = -b_dq[2]; + c_y[12] = -b_dq[3]; + c_y[1] = b_dq[1]; + c_y[5] = b_dq[0]; + c_y[9] = -b_dq[3]; + c_y[13] = b_dq[2]; + c_y[2] = b_dq[2]; + c_y[6] = b_dq[3]; + c_y[10] = b_dq[0]; + c_y[14] = -b_dq[1]; + c_y[3] = b_dq[3]; + c_y[7] = -b_dq[2]; + c_y[11] = b_dq[1]; + c_y[15] = b_dq[0]; + d_y[0] = b_dq[0]; + d_y[1] = -b_dq[1]; + d_y[2] = -b_dq[2]; + d_y[3] = -b_dq[3]; + d_y[4] = b_dq[1]; + d_y[5] = b_dq[0]; + d_y[6] = b_dq[3]; + d_y[7] = -b_dq[2]; + d_y[8] = b_dq[2]; + d_y[9] = -b_dq[3]; + d_y[10] = b_dq[0]; + d_y[11] = b_dq[1]; + d_y[12] = b_dq[3]; + d_y[13] = b_dq[2]; + d_y[14] = -b_dq[1]; + d_y[15] = b_dq[0]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 3; i++) { + dq[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + dq[i0 + (i << 2)] += c_y[i0 + (i1 << 2)] * (float)iv2[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 3; i++) { + c_dq[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + c_dq[i0 + (i << 2)] += d_y[i0 + (i1 << 2)] * dq[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + z_acc[i0] = 0.0F; + for (i = 0; i < 4; i++) { + z_acc[i0] += b_X[i] * c_dq[i + (i0 << 2)]; + } + + gyro_input[i0] = z_acc[i0]; + } + + // Measurement Jacobian + // 'QEKF:160' H_acc = [-devec * (Gamma(Gamma(q_apriori)*[0;0;0;-g])*I_conj + Phi(Phi(q_apriori)'*[0;0;0;-g])), ... + // 'QEKF:161' zeros(3,3), ... + // 'QEKF:162' zeros(3,3)]; + b_X[0] = 0.0F; + b_X[1] = 0.0F; + b_X[2] = 0.0F; + b_X[3] = -g; + c_y[0] = b_dq[0]; + c_y[1] = -b_dq[1]; + c_y[2] = -b_dq[2]; + c_y[3] = -b_dq[3]; + c_y[4] = b_dq[1]; + c_y[5] = b_dq[0]; + c_y[6] = b_dq[3]; + c_y[7] = -b_dq[2]; + c_y[8] = b_dq[2]; + c_y[9] = -b_dq[3]; + c_y[10] = b_dq[0]; + c_y[11] = b_dq[1]; + c_y[12] = b_dq[3]; + c_y[13] = b_dq[2]; + c_y[14] = -b_dq[1]; + c_y[15] = b_dq[0]; + fv1[0] = 0.0F; + fv1[1] = 0.0F; + fv1[2] = 0.0F; + fv1[3] = -g; + d_y[0] = b_dq[0]; + d_y[4] = -b_dq[1]; + d_y[8] = -b_dq[2]; + d_y[12] = -b_dq[3]; + d_y[1] = b_dq[1]; + d_y[5] = b_dq[0]; + d_y[9] = -b_dq[3]; + d_y[13] = b_dq[2]; + d_y[2] = b_dq[2]; + d_y[6] = b_dq[3]; + d_y[10] = b_dq[0]; + d_y[14] = -b_dq[1]; + d_y[3] = b_dq[3]; + d_y[7] = -b_dq[2]; + d_y[11] = b_dq[1]; + d_y[15] = b_dq[0]; + for (i0 = 0; i0 < 4; i0++) { + fv2[i0] = 0.0F; + for (i = 0; i < 4; i++) { + fv2[i0] += b_X[i] * c_y[i + (i0 << 2)]; + } + + b_y[i0] = fv2[i0]; + fv3[i0] = 0.0F; + for (i = 0; i < 4; i++) { + fv3[i0] += fv1[i] * d_y[i + (i0 << 2)]; + } + + q[i0] = fv3[i0]; + } + + y[0] = b_y[0]; + y[1] = -b_y[1]; + y[2] = -b_y[2]; + y[3] = -b_y[3]; + y[4] = b_y[1]; + y[5] = b_y[0]; + y[6] = b_y[3]; + y[7] = -b_y[2]; + y[8] = b_y[2]; + y[9] = -b_y[3]; + y[10] = b_y[0]; + y[11] = b_y[1]; + y[12] = b_y[3]; + y[13] = b_y[2]; + y[14] = -b_y[1]; + y[15] = b_y[0]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 4; i++) { + fv0[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + fv0[i0 + (i << 2)] += (float)iv4[i0 + (i1 << 2)] * y[i1 + (i << 2)]; + } + } + } + + c_y[0] = q[0]; + c_y[1] = -q[1]; + c_y[2] = -q[2]; + c_y[3] = -q[3]; + c_y[4] = q[1]; + c_y[5] = q[0]; + c_y[6] = -q[3]; + c_y[7] = q[2]; + c_y[8] = q[2]; + c_y[9] = q[3]; + c_y[10] = q[0]; + c_y[11] = -q[1]; + c_y[12] = q[3]; + c_y[13] = -q[2]; + c_y[14] = q[1]; + c_y[15] = q[0]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 4; i++) { + d_y[i + (i0 << 2)] = fv0[i + (i0 << 2)] + c_y[i + (i0 << 2)]; + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 3; i++) { + dq[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + dq[i0 + (i << 2)] += d_y[i0 + (i1 << 2)] * (float)iv2[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + for (i = 0; i < 4; i++) { + H_acc[i + 10 * i0] = dq[i + (i0 << 2)]; + } + + for (i = 0; i < 3; i++) { + H_acc[(i + 10 * i0) + 4] = 0.0F; + H_acc[(i + 10 * i0) + 7] = 0.0F; + } + } + } + + // 'QEKF:165' z_hat_gyro = omega_apriori + gyro_bias_apriori; + // 'QEKF:166' H_gyro = [zeros(3,4), ... + // 'QEKF:167' eye(3), ... + // 'QEKF:168' BiasEstimationEnabledMat]; + // 'QEKF:169' z_hat = [z_hat_acc; z_hat_gyro]; + // 'QEKF:170' H = [H_acc; H_gyro]; + for (i = 0; i < 3; i++) { + z_hat[i] = gyro_input[i]; + z_hat[i + 3] = X[i + 4] + gyro_bias[i]; + for (i0 = 0; i0 < 10; i0++) { + H[i0 + 10 * i] = H_acc[i0 + 10 * i]; + } + + for (i0 = 0; i0 < 4; i0++) { + H[i0 + 10 * (i + 3)] = 0.0F; + } + + for (i0 = 0; i0 < 3; i0++) { + H[(i0 + 10 * (i + 3)) + 4] = iv0[i0 + 3 * i]; + H[(i0 + 10 * (i + 3)) + 7] = BiasEstimationEnabledMat[i0 + 3 * i]; + } + } + + // 'QEKF:172' if (UseHeadingForCorrection) + if (UseHeadingForCorrection) { + // 'QEKF:173' z_heading = Heading; + // 'QEKF:174' heading_vector_hat = [zeros(2,1),eye(2),zeros(2,1)] * Phi(q_apriori) * Gamma(q_apriori)' * [0;1;0;0]; + c_y[0] = b_dq[0]; + c_y[1] = -b_dq[1]; + c_y[2] = -b_dq[2]; + c_y[3] = -b_dq[3]; + c_y[4] = b_dq[1]; + c_y[5] = b_dq[0]; + c_y[6] = -b_dq[3]; + c_y[7] = b_dq[2]; + c_y[8] = b_dq[2]; + c_y[9] = b_dq[3]; + c_y[10] = b_dq[0]; + c_y[11] = -b_dq[1]; + c_y[12] = b_dq[3]; + c_y[13] = -b_dq[2]; + c_y[14] = b_dq[1]; + c_y[15] = b_dq[0]; + d_y[0] = b_dq[0]; + d_y[4] = -b_dq[1]; + d_y[8] = -b_dq[2]; + d_y[12] = -b_dq[3]; + d_y[1] = b_dq[1]; + d_y[5] = b_dq[0]; + d_y[9] = b_dq[3]; + d_y[13] = -b_dq[2]; + d_y[2] = b_dq[2]; + d_y[6] = -b_dq[3]; + d_y[10] = b_dq[0]; + d_y[14] = b_dq[1]; + d_y[3] = b_dq[3]; + d_y[7] = b_dq[2]; + d_y[11] = -b_dq[1]; + d_y[15] = b_dq[0]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 2; i++) { + d_dq[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + d_dq[i0 + (i << 2)] += c_y[i0 + (i1 << 2)] * (float)iv5[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 2; i++) { + e_dq[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + e_dq[i0 + (i << 2)] += d_y[i0 + (i1 << 2)] * d_dq[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 2; i0++) { + heading_vector_hat[i0] = 0.0F; + for (i = 0; i < 4; i++) { + heading_vector_hat[i0] += (float)iv6[i] * e_dq[i + (i0 << 2)]; + } + + b_heading_vector_hat[i0] = heading_vector_hat[i0]; + } + + // estimated measurement + // 'QEKF:175' z_hat_heading = atan2(heading_vector_hat(2), heading_vector_hat(1)); + // d atan2(y,x) / dx = -y / (x^2+y^2) + // d atan2(y,x) / dy = x / (x^2+y^2) + // d heading_hat / d q = (d atan2(y,x) / dx)*(d heading_vector_hat(1) / dq) + (d atan2(y,x) / dy)*(d heading_vector_hat(2) / dq) + // 'QEKF:181' H_heading_vector = [[zeros(2,1),eye(2),zeros(2,1)] * (Phi(Phi(q_apriori)*[0;1;0;0])*I_conj + Gamma(Gamma(q_apriori)'*[0;1;0;0])), zeros(2,3), zeros(2,3)]; + c_y[0] = b_dq[0]; + c_y[1] = -b_dq[1]; + c_y[2] = -b_dq[2]; + c_y[3] = -b_dq[3]; + c_y[4] = b_dq[1]; + c_y[5] = b_dq[0]; + c_y[6] = -b_dq[3]; + c_y[7] = b_dq[2]; + c_y[8] = b_dq[2]; + c_y[9] = b_dq[3]; + c_y[10] = b_dq[0]; + c_y[11] = -b_dq[1]; + c_y[12] = b_dq[3]; + c_y[13] = -b_dq[2]; + c_y[14] = b_dq[1]; + c_y[15] = b_dq[0]; + d_y[0] = b_dq[0]; + d_y[4] = -b_dq[1]; + d_y[8] = -b_dq[2]; + d_y[12] = -b_dq[3]; + d_y[1] = b_dq[1]; + d_y[5] = b_dq[0]; + d_y[9] = b_dq[3]; + d_y[13] = -b_dq[2]; + d_y[2] = b_dq[2]; + d_y[6] = -b_dq[3]; + d_y[10] = b_dq[0]; + d_y[14] = b_dq[1]; + d_y[3] = b_dq[3]; + d_y[7] = b_dq[2]; + d_y[11] = -b_dq[1]; + d_y[15] = b_dq[0]; + for (i0 = 0; i0 < 4; i0++) { + b_X[i0] = 0.0F; + for (i = 0; i < 4; i++) { + b_X[i0] += (float)iv6[i] * c_y[i + (i0 << 2)]; + } + + b_y[i0] = b_X[i0]; + fv2[i0] = 0.0F; + for (i = 0; i < 4; i++) { + fv2[i0] += (float)iv6[i] * d_y[i + (i0 << 2)]; + } + + q[i0] = fv2[i0]; + } + + // 'QEKF:182' dAtan2dxy = [-heading_vector_hat(2) / (heading_vector_hat(1)^2+heading_vector_hat(2)^2), ... + // 'QEKF:183' heading_vector_hat(1) / (heading_vector_hat(1)^2+heading_vector_hat(2)^2)]; + // 'QEKF:184' H_heading = dAtan2dxy * H_heading_vector; + // Heading measurement covariance + // 'QEKF:187' cov_heading = sigma2_heading; + // 'QEKF:189' z2 = [z; z_heading]; + // 'QEKF:190' z2_hat = [z_hat; z_hat_heading]; + // 'QEKF:191' H2 = [H; H_heading]; + y[0] = b_y[0]; + y[1] = -b_y[1]; + y[2] = -b_y[2]; + y[3] = -b_y[3]; + y[4] = b_y[1]; + y[5] = b_y[0]; + y[6] = -b_y[3]; + y[7] = b_y[2]; + y[8] = b_y[2]; + y[9] = b_y[3]; + y[10] = b_y[0]; + y[11] = -b_y[1]; + y[12] = b_y[3]; + y[13] = -b_y[2]; + y[14] = b_y[1]; + y[15] = b_y[0]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 4; i++) { + fv0[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + fv0[i0 + (i << 2)] += (float)iv4[i0 + (i1 << 2)] * y[i1 + (i << 2)]; + } + } + } + + c_y[0] = q[0]; + c_y[1] = -q[1]; + c_y[2] = -q[2]; + c_y[3] = -q[3]; + c_y[4] = q[1]; + c_y[5] = q[0]; + c_y[6] = q[3]; + c_y[7] = -q[2]; + c_y[8] = q[2]; + c_y[9] = -q[3]; + c_y[10] = q[0]; + c_y[11] = q[1]; + c_y[12] = q[3]; + c_y[13] = q[2]; + c_y[14] = -q[1]; + c_y[15] = q[0]; + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 4; i++) { + d_y[i + (i0 << 2)] = fv0[i + (i0 << 2)] + c_y[i + (i0 << 2)]; + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (i = 0; i < 2; i++) { + d_dq[i0 + (i << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + d_dq[i0 + (i << 2)] += d_y[i0 + (i1 << 2)] * (float)iv5[i1 + (i << 2)]; + } + } + } + + for (i0 = 0; i0 < 2; i0++) { + for (i = 0; i < 4; i++) { + fv4[i + 10 * i0] = d_dq[i + (i0 << 2)]; + } + + for (i = 0; i < 3; i++) { + fv4[(i + 10 * i0) + 4] = 0.0F; + fv4[(i + 10 * i0) + 7] = 0.0F; + } + } + + heading_vector_hat[0] = -b_heading_vector_hat[1] / (b_heading_vector_hat[0] * + b_heading_vector_hat[0] + b_heading_vector_hat[1] * b_heading_vector_hat[1]); + heading_vector_hat[1] = b_heading_vector_hat[0] / (b_heading_vector_hat[0] * + b_heading_vector_hat[0] + b_heading_vector_hat[1] * b_heading_vector_hat[1]); + for (i0 = 0; i0 < 10; i0++) { + c_z[i0] = 0.0F; + for (i = 0; i < 2; i++) { + c_z[i0] += fv4[i0 + 10 * i] * heading_vector_hat[i]; + } + } + + for (i0 = 0; i0 < 6; i0++) { + for (i = 0; i < 10; i++) { + H2[i + 10 * i0] = H[i + 10 * i0]; + } + } + + for (i0 = 0; i0 < 10; i0++) { + H2[60 + i0] = c_z[i0]; + } + + // 'QEKF:192' R2 = [R, zeros(6,1); zeros(1,6), cov_heading]; + // Calculate Kalman gain + // 'QEKF:195' S2 = H2 * P_apriori * H2' + R2; + // 'QEKF:196' K2 = P_apriori * H2' / S2; + for (i0 = 0; i0 < 7; i0++) { + for (i = 0; i < 10; i++) { + b_H2[i0 + 7 * i] = 0.0F; + for (i1 = 0; i1 < 10; i1++) { + b_H2[i0 + 7 * i] += H2[i1 + 10 * i0] * P_apriori[i1 + 10 * i]; + } + } + } + + for (i0 = 0; i0 < 10; i0++) { + for (i = 0; i < 7; i++) { + K2[i0 + 10 * i] = 0.0F; + for (i1 = 0; i1 < 10; i1++) { + K2[i0 + 10 * i] += P_apriori[i0 + 10 * i1] * H2[i1 + 10 * i]; + } + } + } + + for (i0 = 0; i0 < 7; i0++) { + for (i = 0; i < 7; i++) { + c_H2[i0 + 7 * i] = 0.0F; + for (i1 = 0; i1 < 10; i1++) { + c_H2[i0 + 7 * i] += H2[i1 + 10 * i0] * K2[i1 + 10 * i]; + } + } + } + + for (i0 = 0; i0 < 6; i0++) { + for (i = 0; i < 6; i++) { + b_R[i + 7 * i0] = R[i + 6 * i0]; + } + + b_R[6 + 7 * i0] = 0.0F; + b_R[42 + i0] = 0.0F; + } + + b_R[48] = sigma2_heading; + for (i0 = 0; i0 < 7; i0++) { + for (i = 0; i < 7; i++) { + d_H2[i + 7 * i0] = c_H2[i + 7 * i0] + b_R[i + 7 * i0]; + } + } + + mrdivide(b_H2, d_H2, K2); + + // P_apriori * H2' * inv(S2) + // if (NormalizeAccelerometer && (norm_acc <= 0)) + // K2 = single(zeros(size(K2))); % if there is no accelerometer measurement, then no correction will be performed + // end + // Correct using innovation + // 'QEKF:203' X_aposteriori = X_apriori + K2 * (z2 - z2_hat); + d_z[6] = Heading; + for (i0 = 0; i0 < 6; i0++) { + d_z[i0] = z[i0]; + b_z_hat[i0] = z_hat[i0]; + } + + b_z_hat[6] = rt_atan2f_snf(b_heading_vector_hat[1], b_heading_vector_hat[0]); + for (i0 = 0; i0 < 7; i0++) { + e_z[i0] = d_z[i0] - b_z_hat[i0]; + } + + // 'QEKF:204' P_aposteriori = (eye(10) - K2*H2) * P_apriori; + c_eye(dv2); + for (i0 = 0; i0 < 10; i0++) { + c_z[i0] = 0.0F; + for (i = 0; i < 7; i++) { + c_z[i0] += e_z[i] * K2[i + 7 * i0]; + } + + X_out[i0] = X_apriori[i0] + c_z[i0]; + for (i = 0; i < 10; i++) { + coeff_b = 0.0F; + for (i1 = 0; i1 < 7; i1++) { + coeff_b += H2[i0 + 10 * i1] * K2[i1 + 7 * i]; + } + + Q[i0 + 10 * i] = (float)dv2[i0 + 10 * i] - coeff_b; + } + } + + for (i0 = 0; i0 < 10; i0++) { + for (i = 0; i < 10; i++) { + P_out[i0 + 10 * i] = 0.0F; + for (i1 = 0; i1 < 10; i1++) { + P_out[i0 + 10 * i] += P_apriori[i0 + 10 * i1] * Q[i1 + 10 * i]; + } + } + } + } else { + // 'QEKF:205' else + // Calculate Kalman gain + // 'QEKF:207' S = H * P_apriori * H' + R; + // 'QEKF:208' K = P_apriori * H' / S; + for (i0 = 0; i0 < 6; i0++) { + for (i = 0; i < 10; i++) { + b_H[i0 + 6 * i] = 0.0F; + for (i1 = 0; i1 < 10; i1++) { + b_H[i0 + 6 * i] += H[i1 + 10 * i0] * P_apriori[i1 + 10 * i]; + } + } + } + + for (i0 = 0; i0 < 10; i0++) { + for (i = 0; i < 6; i++) { + K[i0 + 10 * i] = 0.0F; + for (i1 = 0; i1 < 10; i1++) { + K[i0 + 10 * i] += P_apriori[i0 + 10 * i1] * H[i1 + 10 * i]; + } + } + } + + for (i0 = 0; i0 < 6; i0++) { + for (i = 0; i < 6; i++) { + coeff_b = 0.0F; + for (i1 = 0; i1 < 10; i1++) { + coeff_b += H[i1 + 10 * i0] * K[i1 + 10 * i]; + } + + c_H[i0 + 6 * i] = coeff_b + R[i0 + 6 * i]; + } + } + + b_mrdivide(b_H, c_H, K); + + // P_apriori * H' * inv(S) + // if (NormalizeAccelerometer && (norm_acc <= 0)) + // K = single(zeros(size(K))); % if there is no accelerometer measurement, then no correction will be performed + // end + // 'QEKF:214' X_aposteriori = X_apriori + K * (z - z_hat); + for (i0 = 0; i0 < 6; i0++) { + b_z[i0] = z[i0] - z_hat[i0]; + } + + // 'QEKF:215' P_aposteriori = (eye(10) - K*H) * P_apriori; + c_eye(dv2); + for (i0 = 0; i0 < 10; i0++) { + c_z[i0] = 0.0F; + for (i = 0; i < 6; i++) { + c_z[i0] += b_z[i] * K[i + 6 * i0]; + } + + X_out[i0] = X_apriori[i0] + c_z[i0]; + for (i = 0; i < 10; i++) { + coeff_b = 0.0F; + for (i1 = 0; i1 < 6; i1++) { + coeff_b += H[i0 + 10 * i1] * K[i1 + 6 * i]; + } + + Q[i0 + 10 * i] = (float)dv2[i0 + 10 * i] - coeff_b; + } + } + + for (i0 = 0; i0 < 10; i0++) { + for (i = 0; i < 10; i++) { + P_out[i0 + 10 * i] = 0.0F; + for (i1 = 0; i1 < 10; i1++) { + P_out[i0 + 10 * i] += P_apriori[i0 + 10 * i1] * Q[i1 + 10 * i]; + } + } + } + } + + // %% Normalize quaternion + // 'QEKF:219' X_aposteriori(1:4) = X_aposteriori(1:4) / norm(X_aposteriori(1:4)); + coeff_b = b_norm(*(float (*)[4])&X_out[0]); + for (i0 = 0; i0 < 4; i0++) { + X_out[i0] /= coeff_b; + } + + // %% Send output to Simulink + // 'QEKF:222' X_out = X_aposteriori; + // 'QEKF:223' P_out = P_aposteriori; +} + +// +// Arguments : void +// Return Type : void +// +void acc_norm_filtered_not_empty_init() +{ + acc_norm_filtered_not_empty = false; +} + +// +// Arguments : void +// Return Type : void +// +void acc_norm_old_not_empty_init() +{ + acc_norm_old_not_empty = false; +} + +// +// File trailer for QEKF.cpp +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/QEKF.h b/Estimators/QEKF/codegen/lib/QEKF/QEKF.h new file mode 100644 index 0000000..d38357c --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/QEKF.h @@ -0,0 +1,41 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: QEKF.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// +#ifndef QEKF_H +#define QEKF_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "QEKF_types.h" + +// Function Declarations +extern void QEKF(const float X[10], const float P_prev[100], const float + Gyroscope[3], const float Accelerometer[3], float Heading, + boolean_T UseHeadingForCorrection, float SamplePeriod, + boolean_T SensorDriven, boolean_T BiasEstimationEnabled, + boolean_T YawBiasEstimationEnabled, boolean_T + NormalizeAccelerometer, const float cov_gyro[9], const float + cov_acc[9], float GyroscopeTrustFactor, float sigma2_omega, + float sigma2_heading, float sigma2_bias, boolean_T + AccelerometerVibrationDetectionEnabled, float + AccelerometerVibrationNormLPFtau, float + AccelerometerVibrationCovarianceVaryFactor, float MaxVaryFactor, + float g, float X_out[10], float P_out[100]); +extern void acc_norm_filtered_not_empty_init(); +extern void acc_norm_old_not_empty_init(); + +#endif + +// +// File trailer for QEKF.h +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/QEKF_initialize.cpp b/Estimators/QEKF/codegen/lib/QEKF/QEKF_initialize.cpp new file mode 100644 index 0000000..f0f7152 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/QEKF_initialize.cpp @@ -0,0 +1,33 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: QEKF_initialize.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// + +// Include Files +#include "rt_nonfinite.h" +#include "QEKF.h" +#include "QEKF_initialize.h" + +// Function Definitions + +// +// Arguments : void +// Return Type : void +// +void QEKF_initialize() +{ + rt_InitInfAndNaN(8U); + acc_norm_old_not_empty_init(); + acc_norm_filtered_not_empty_init(); +} + +// +// File trailer for QEKF_initialize.cpp +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/QEKF_initialize.h b/Estimators/QEKF/codegen/lib/QEKF/QEKF_initialize.h new file mode 100644 index 0000000..03b3154 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/QEKF_initialize.h @@ -0,0 +1,28 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: QEKF_initialize.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// +#ifndef QEKF_INITIALIZE_H +#define QEKF_INITIALIZE_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "QEKF_types.h" + +// Function Declarations +extern void QEKF_initialize(); + +#endif + +// +// File trailer for QEKF_initialize.h +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/QEKF_ref.rsp b/Estimators/QEKF/codegen/lib/QEKF/QEKF_ref.rsp new file mode 100644 index 0000000..e69de29 diff --git a/Estimators/QEKF/codegen/lib/QEKF/QEKF_rtw.bat b/Estimators/QEKF/codegen/lib/QEKF/QEKF_rtw.bat new file mode 100644 index 0000000..2b197e2 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/QEKF_rtw.bat @@ -0,0 +1,12 @@ +@echo off + +cd . + +if "%1"=="" ("C:\PROGRA~1\MATLAB\R2018a\bin\win64\gmake" -f QEKF_rtw.mk all) else ("C:\PROGRA~1\MATLAB\R2018a\bin\win64\gmake" -f QEKF_rtw.mk %1) +@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/Estimators/QEKF/codegen/lib/QEKF/QEKF_rtw.mk b/Estimators/QEKF/codegen/lib/QEKF/QEKF_rtw.mk new file mode 100644 index 0000000..1de0529 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/QEKF_rtw.mk @@ -0,0 +1,553 @@ +########################################################################### +## Makefile generated for MATLAB file/project 'QEKF'. +## +## Makefile : QEKF_rtw.mk +## Generated on : Fri Apr 26 13:23:17 2019 +## MATLAB Coder version: 4.0 (R2018a) +## +## Build Info: +## +## Final product: $(RELATIVE_PATH_TO_ANCHOR)/QEKF.lib +## Product type : static-library +## +########################################################################### + +########################################################################### +## MACROS +########################################################################### + +# Macro Descriptions: +# PRODUCT_NAME Name of the system to build +# MAKEFILE Name of this makefile +# COMPUTER Computer type. See the MATLAB "computer" command. + +PRODUCT_NAME = QEKF +MAKEFILE = QEKF_rtw.mk +COMPUTER = PCWIN64 +MATLAB_ROOT = C:/PROGRA~1/MATLAB/R2018a +MATLAB_BIN = C:/PROGRA~1/MATLAB/R2018a/bin +MATLAB_ARCH_BIN = $(MATLAB_BIN)/win64 +MASTER_ANCHOR_DIR = +START_DIR = C:/Users/Thomas/Documents/Kugle-MATLAB/Estimators/QEKF +ARCH = win64 +RELATIVE_PATH_TO_ANCHOR = . +C_STANDARD_OPTS = +CPP_STANDARD_OPTS = + +########################################################################### +## TOOLCHAIN SPECIFICATIONS +########################################################################### + +# Toolchain Name: GNU Tools for ARM Embedded Processors v5.2 | gmake (64-bit Windows) +# Supported Version(s): +# ToolchainInfo Version: R2018a +# Specification Revision: 1.0 +# +#------------------------------------------- +# Macros assumed to be defined elsewhere +#------------------------------------------- + +# TARGET_LOAD_CMD_ARGS +# TARGET_LOAD_CMD +# MW_GNU_ARM_TOOLS_PATH +# FDATASECTIONS_FLG + +#----------- +# MACROS +#----------- + +LIBGCC = ${shell arm-none-eabi-gcc ${CFLAGS} -print-libgcc-file-name} +LIBC = ${shell arm-none-eabi-gcc ${CFLAGS} -print-file-name=libc.a} +LIBM = ${shell arm-none-eabi-gcc ${CFLAGS} -print-file-name=libm.a} +PRODUCT_BIN = $(RELATIVE_PATH_TO_ANCHOR)/$(PRODUCT_NAME).bin +PRODUCT_HEX = $(RELATIVE_PATH_TO_ANCHOR)/$(PRODUCT_NAME).hex +CPFLAGS = -O binary +SHELL = %SystemRoot%/system32/cmd.exe + +TOOLCHAIN_SRCS = +TOOLCHAIN_INCS = +TOOLCHAIN_LIBS = -lm + +#------------------------ +# BUILD TOOL COMMANDS +#------------------------ + +# Assembler: GNU ARM Assembler +AS_PATH = $(MW_GNU_ARM_TOOLS_PATH) +AS = $(AS_PATH)/arm-none-eabi-gcc + +# C Compiler: GNU ARM C Compiler +CC_PATH = $(MW_GNU_ARM_TOOLS_PATH) +CC = $(CC_PATH)/arm-none-eabi-gcc + +# Linker: GNU ARM Linker +LD_PATH = $(MW_GNU_ARM_TOOLS_PATH) +LD = $(LD_PATH)/arm-none-eabi-g++ + +# C++ Compiler: GNU ARM C++ Compiler +CPP_PATH = $(MW_GNU_ARM_TOOLS_PATH) +CPP = $(CPP_PATH)/arm-none-eabi-g++ + +# C++ Linker: GNU ARM C++ Linker +CPP_LD_PATH = $(MW_GNU_ARM_TOOLS_PATH) +CPP_LD = $(CPP_LD_PATH)/arm-none-eabi-g++ + +# Archiver: GNU ARM Archiver +AR_PATH = $(MW_GNU_ARM_TOOLS_PATH) +AR = $(AR_PATH)/arm-none-eabi-ar + +# MEX Tool: MEX Tool +MEX_PATH = $(MATLAB_ARCH_BIN) +MEX = $(MEX_PATH)/mex + +# Binary Converter: Binary Converter +OBJCOPYPATH = $(MW_GNU_ARM_TOOLS_PATH) +OBJCOPY = $(OBJCOPYPATH)/arm-none-eabi-objcopy + +# Hex Converter: Hex Converter +OBJCOPYPATH = $(MW_GNU_ARM_TOOLS_PATH) +OBJCOPY = $(OBJCOPYPATH)/arm-none-eabi-objcopy + +# Executable Size: Executable Size +EXESIZEPATH = $(MW_GNU_ARM_TOOLS_PATH) +EXESIZE = $(EXESIZEPATH)/arm-none-eabi-size + +# Download: Download +DOWNLOAD = + +# Execute: Execute +EXECUTE = $(PRODUCT) + +# Builder: GMAKE Utility +MAKE_PATH = %MATLAB%\bin\win64 +MAKE = $(MAKE_PATH)/gmake + + +#------------------------- +# Directives/Utilities +#------------------------- + +ASDEBUG = -g +AS_OUTPUT_FLAG = -o +CDEBUG = -g +C_OUTPUT_FLAG = -o +LDDEBUG = -g +OUTPUT_FLAG = -o +CPPDEBUG = -g +CPP_OUTPUT_FLAG = -o +CPPLDDEBUG = -g +OUTPUT_FLAG = -o +ARDEBUG = +STATICLIB_OUTPUT_FLAG = +MEX_DEBUG = -g +RM = @del /f/q +ECHO = @echo +MV = @move +RUN = + +#---------------------------------------- +# "Faster Builds" Build Configuration +#---------------------------------------- + +ARFLAGS = ruvs +ASFLAGS = -MMD -MP -MF"$(@:%.o=%.dep)" -MT"$@" \ + -Wall \ + -x assembler-with-cpp \ + $(ASFLAGS_ADDITIONAL) \ + $(DEFINES) \ + $(INCLUDES) \ + -c +OBJCOPYFLAGS_BIN = -O binary $(PRODUCT) $(PRODUCT_BIN) +CFLAGS = $(FDATASECTIONS_FLG) \ + -Wall \ + -MMD -MP -MF"$(@:%.o=%.dep)" -MT"$@" \ + -c \ + -O0 +CPPFLAGS = -std=c++98 \ + -fno-rtti \ + -fno-exceptions \ + $(FDATASECTIONS_FLG) \ + -Wall \ + -MMD -MP -MF"$(@:%.o=%.dep)" -MT"$@" \ + -c \ + -O0 +CPP_LDFLAGS = -Wl,--gc-sections \ + -Wl,-Map="$(PRODUCT_NAME).map" +CPP_SHAREDLIB_LDFLAGS = +DOWNLOAD_FLAGS = +EXESIZE_FLAGS = $(PRODUCT) +EXECUTE_FLAGS = +OBJCOPYFLAGS_HEX = -O ihex $(PRODUCT) $(PRODUCT_HEX) +LDFLAGS = -Wl,--gc-sections \ + -Wl,-Map="$(PRODUCT_NAME).map" +MEX_CPPFLAGS = +MEX_CPPLDFLAGS = +MEX_CFLAGS = +MEX_LDFLAGS = +MAKE_FLAGS = -f $(MAKEFILE) +SHAREDLIB_LDFLAGS = + +#-------------------- +# File extensions +#-------------------- + +ASM_Type1_Ext = .S +DEP_EXT = .dep +OBJ_EXT = .o +ASM_EXT = .s +DEP_EXT = .dep +H_EXT = .h +OBJ_EXT = .o +C_EXT = .c +EXE_EXT = .elf +SHAREDLIB_EXT = .so +CXX_EXT = .cxx +DEP_EXT = .dep +HPP_EXT = .hpp +OBJ_EXT = .o +CPP_EXT = .cpp +UNIX_TYPE1_EXT = .cc +UNIX_TYPE2_EXT = .C +EXE_EXT = .elf +SHAREDLIB_EXT = .so +STATICLIB_EXT = .lib +MEX_EXT = .mexw64 +MAKE_EXT = .mk + + +########################################################################### +## OUTPUT INFO +########################################################################### + +PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)/QEKF.lib +PRODUCT_TYPE = "static-library" +BUILD_TYPE = "Static Library" + +########################################################################### +## INCLUDE PATHS +########################################################################### + +INCLUDES_BUILDINFO = -I$(START_DIR)/codegen/lib/QEKF -I$(START_DIR) -I$(MATLAB_ROOT)/extern/include -I$(MATLAB_ROOT)/simulink/include -I$(MATLAB_ROOT)/rtw/c/src -I$(MATLAB_ROOT)/rtw/c/src/ext_mode/common -I$(MATLAB_ROOT)/rtw/c/ert + +INCLUDES = $(INCLUDES_BUILDINFO) + +########################################################################### +## DEFINES +########################################################################### + +DEFINES_STANDARD = -DMODEL=QEKF -DHAVESTDIO -DUSE_RTMODEL + +DEFINES = $(DEFINES_STANDARD) + +########################################################################### +## SOURCE FILES +########################################################################### + +SRCS = $(START_DIR)/codegen/lib/QEKF/QEKF_initialize.cpp $(START_DIR)/codegen/lib/QEKF/QEKF_terminate.cpp $(START_DIR)/codegen/lib/QEKF/QEKF.cpp $(START_DIR)/codegen/lib/QEKF/eye.cpp $(START_DIR)/codegen/lib/QEKF/mrdivide.cpp $(START_DIR)/codegen/lib/QEKF/norm.cpp $(START_DIR)/codegen/lib/QEKF/rt_nonfinite.cpp $(START_DIR)/codegen/lib/QEKF/rtGetNaN.cpp $(START_DIR)/codegen/lib/QEKF/rtGetInf.cpp + +ALL_SRCS = $(SRCS) + +########################################################################### +## OBJECTS +########################################################################### + +OBJS = QEKF_initialize.o QEKF_terminate.o QEKF.o eye.o mrdivide.o norm.o rt_nonfinite.o rtGetNaN.o rtGetInf.o + +ALL_OBJS = $(OBJS) + +########################################################################### +## PREBUILT OBJECT FILES +########################################################################### + +PREBUILT_OBJS = + +########################################################################### +## LIBRARIES +########################################################################### + +LIBS = + +########################################################################### +## SYSTEM LIBRARIES +########################################################################### + +SYSTEM_LIBS = + +########################################################################### +## ADDITIONAL TOOLCHAIN FLAGS +########################################################################### + +#--------------- +# C Compiler +#--------------- + +CFLAGS_BASIC = $(DEFINES) $(INCLUDES) + +CFLAGS += $(CFLAGS_BASIC) + +#----------------- +# C++ Compiler +#----------------- + +CPPFLAGS_BASIC = $(DEFINES) $(INCLUDES) + +CPPFLAGS += $(CPPFLAGS_BASIC) + +########################################################################### +## INLINED COMMANDS +########################################################################### + + +ALL_DEPS:=$(patsubst %$(OBJ_EXT),%$(DEP_EXT),$(ALL_OBJS)) +all: + +ifndef DISABLE_GCC_FUNCTION_DATA_SECTIONS +FDATASECTIONS_FLG := -ffunction-sections -fdata-sections +endif + + + +-include codertarget_assembly_flags.mk +-include ../codertarget_assembly_flags.mk +-include $(ALL_DEPS) + + +########################################################################### +## PHONY TARGETS +########################################################################### + +.PHONY : all build clean info prebuild postbuild download execute + + +all : build postbuild + @echo "### Successfully generated all binary outputs." + + +build : prebuild $(PRODUCT) + + +prebuild : + + +postbuild : build + + +download : postbuild + + +execute : download + + +########################################################################### +## FINAL TARGET +########################################################################### + +#--------------------------------- +# Create a static library +#--------------------------------- + +$(PRODUCT) : $(OBJS) $(PREBUILT_OBJS) + @echo "### Creating static library "$(PRODUCT)" ..." + $(AR) $(ARFLAGS) $(PRODUCT) $(OBJS) + @echo "### Created: $(PRODUCT)" + + +########################################################################### +## INTERMEDIATE TARGETS +########################################################################### + +#--------------------- +# SOURCE-TO-OBJECT +#--------------------- + +%.o : %.c + $(CC) $(CFLAGS) -o $@ $< + + +%.o : %.s + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : %.S + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : %.cpp + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : %.cc + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : %.C + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : %.cxx + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.c + $(CC) $(CFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.s + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.S + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.cpp + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.cc + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.C + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.cxx + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.c + $(CC) $(CFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.s + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.S + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.cpp + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.cc + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.C + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.cxx + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/QEKF/%.c + $(CC) $(CFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/QEKF/%.s + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/QEKF/%.S + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/QEKF/%.cpp + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/QEKF/%.cc + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/QEKF/%.C + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/QEKF/%.cxx + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.c + $(CC) $(CFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.s + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.S + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.cpp + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.cc + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.C + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.cxx + $(CPP) $(CPPFLAGS) -o $@ $< + + +########################################################################### +## DEPENDENCIES +########################################################################### + +$(ALL_OBJS) : $(MAKEFILE) rtw_proj.tmw + + +########################################################################### +## MISCELLANEOUS TARGETS +########################################################################### + +info : + @echo "### PRODUCT = $(PRODUCT)" + @echo "### PRODUCT_TYPE = $(PRODUCT_TYPE)" + @echo "### BUILD_TYPE = $(BUILD_TYPE)" + @echo "### INCLUDES = $(INCLUDES)" + @echo "### DEFINES = $(DEFINES)" + @echo "### ALL_SRCS = $(ALL_SRCS)" + @echo "### ALL_OBJS = $(ALL_OBJS)" + @echo "### LIBS = $(LIBS)" + @echo "### MODELREF_LIBS = $(MODELREF_LIBS)" + @echo "### SYSTEM_LIBS = $(SYSTEM_LIBS)" + @echo "### TOOLCHAIN_LIBS = $(TOOLCHAIN_LIBS)" + @echo "### ASFLAGS = $(ASFLAGS)" + @echo "### CFLAGS = $(CFLAGS)" + @echo "### LDFLAGS = $(LDFLAGS)" + @echo "### SHAREDLIB_LDFLAGS = $(SHAREDLIB_LDFLAGS)" + @echo "### CPPFLAGS = $(CPPFLAGS)" + @echo "### CPP_LDFLAGS = $(CPP_LDFLAGS)" + @echo "### CPP_SHAREDLIB_LDFLAGS = $(CPP_SHAREDLIB_LDFLAGS)" + @echo "### ARFLAGS = $(ARFLAGS)" + @echo "### MEX_CFLAGS = $(MEX_CFLAGS)" + @echo "### MEX_CPPFLAGS = $(MEX_CPPFLAGS)" + @echo "### MEX_LDFLAGS = $(MEX_LDFLAGS)" + @echo "### MEX_CPPLDFLAGS = $(MEX_CPPLDFLAGS)" + @echo "### OBJCOPYFLAGS_BIN = $(OBJCOPYFLAGS_BIN)" + @echo "### OBJCOPYFLAGS_HEX = $(OBJCOPYFLAGS_HEX)" + @echo "### EXESIZE_FLAGS = $(EXESIZE_FLAGS)" + @echo "### DOWNLOAD_FLAGS = $(DOWNLOAD_FLAGS)" + @echo "### EXECUTE_FLAGS = $(EXECUTE_FLAGS)" + @echo "### MAKE_FLAGS = $(MAKE_FLAGS)" + + +clean : + $(ECHO) "### Deleting all derived files..." + $(RM) $(subst /,\,$(PRODUCT)) + $(RM) $(subst /,\,$(ALL_OBJS)) + $(RM) *.dep + $(ECHO) "### Deleted all derived files." + + diff --git a/Estimators/QEKF/codegen/lib/QEKF/QEKF_terminate.cpp b/Estimators/QEKF/codegen/lib/QEKF/QEKF_terminate.cpp new file mode 100644 index 0000000..7a9c9e4 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/QEKF_terminate.cpp @@ -0,0 +1,31 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: QEKF_terminate.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// + +// Include Files +#include "rt_nonfinite.h" +#include "QEKF.h" +#include "QEKF_terminate.h" + +// Function Definitions + +// +// Arguments : void +// Return Type : void +// +void QEKF_terminate() +{ + // (no terminate code required) +} + +// +// File trailer for QEKF_terminate.cpp +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/QEKF_terminate.h b/Estimators/QEKF/codegen/lib/QEKF/QEKF_terminate.h new file mode 100644 index 0000000..f693bcb --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/QEKF_terminate.h @@ -0,0 +1,28 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: QEKF_terminate.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// +#ifndef QEKF_TERMINATE_H +#define QEKF_TERMINATE_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "QEKF_types.h" + +// Function Declarations +extern void QEKF_terminate(); + +#endif + +// +// File trailer for QEKF_terminate.h +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/QEKF_types.h b/Estimators/QEKF/codegen/lib/QEKF/QEKF_types.h new file mode 100644 index 0000000..2aa4c78 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/QEKF_types.h @@ -0,0 +1,21 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: QEKF_types.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// +#ifndef QEKF_TYPES_H +#define QEKF_TYPES_H + +// Include Files +#include "rtwtypes.h" +#endif + +// +// File trailer for QEKF_types.h +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/buildInfo.mat b/Estimators/QEKF/codegen/lib/QEKF/buildInfo.mat new file mode 100644 index 0000000..502034d Binary files /dev/null and b/Estimators/QEKF/codegen/lib/QEKF/buildInfo.mat differ diff --git a/Estimators/QEKF/codegen/lib/QEKF/codeInfo.mat b/Estimators/QEKF/codegen/lib/QEKF/codeInfo.mat new file mode 100644 index 0000000..06803c0 Binary files /dev/null and b/Estimators/QEKF/codegen/lib/QEKF/codeInfo.mat differ diff --git a/Estimators/QEKF/codegen/lib/QEKF/codedescriptor.dmr b/Estimators/QEKF/codegen/lib/QEKF/codedescriptor.dmr new file mode 100644 index 0000000..6e984ec Binary files /dev/null and b/Estimators/QEKF/codegen/lib/QEKF/codedescriptor.dmr differ diff --git a/Estimators/QEKF/codegen/lib/QEKF/examples/main.cpp b/Estimators/QEKF/codegen/lib/QEKF/examples/main.cpp new file mode 100644 index 0000000..06c7fb3 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/examples/main.cpp @@ -0,0 +1,213 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: main.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// + +//*********************************************************************** +// This automatically generated example C main file shows how to call +// entry-point functions that MATLAB Coder generated. You must customize +// this file for your application. Do not modify this file directly. +// Instead, make a copy of this file, modify it, and integrate it into +// your development environment. +// +// This file initializes entry-point function arguments to a default +// size and value before calling the entry-point functions. It does +// not store or use any values returned from the entry-point functions. +// If necessary, it does pre-allocate memory for returned values. +// You can use this file as a starting point for a main function that +// you can deploy in your application. +// +// After you copy the file, and before you deploy it, you must make the +// following changes: +// * For variable-size function arguments, change the example sizes to +// the sizes that your application requires. +// * Change the example values of function arguments to the values that +// your application requires. +// * If the entry-point functions return values, store these values or +// otherwise use them as required by your application. +// +//*********************************************************************** +// Include Files +#include "rt_nonfinite.h" +#include "QEKF.h" +#include "main.h" +#include "QEKF_terminate.h" +#include "QEKF_initialize.h" + +// Function Declarations +static void argInit_10x10_real32_T(float result[100]); +static void argInit_10x1_real32_T(float result[10]); +static void argInit_3x1_real32_T(float result[3]); +static void argInit_3x3_real32_T(float result[9]); +static boolean_T argInit_boolean_T(); +static float argInit_real32_T(); +static void main_QEKF(); + +// Function Definitions + +// +// Arguments : float result[100] +// Return Type : void +// +static void argInit_10x10_real32_T(float result[100]) +{ + int idx0; + int idx1; + + // Loop over the array to initialize each element. + for (idx0 = 0; idx0 < 10; idx0++) { + for (idx1 = 0; idx1 < 10; idx1++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx1 + 10 * idx0] = argInit_real32_T(); + } + } +} + +// +// Arguments : float result[10] +// Return Type : void +// +static void argInit_10x1_real32_T(float result[10]) +{ + int idx0; + + // Loop over the array to initialize each element. + for (idx0 = 0; idx0 < 10; idx0++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx0] = argInit_real32_T(); + } +} + +// +// Arguments : float result[3] +// Return Type : void +// +static void argInit_3x1_real32_T(float result[3]) +{ + int idx0; + + // Loop over the array to initialize each element. + for (idx0 = 0; idx0 < 3; idx0++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx0] = argInit_real32_T(); + } +} + +// +// Arguments : float result[9] +// Return Type : void +// +static void argInit_3x3_real32_T(float result[9]) +{ + int idx0; + int idx1; + + // Loop over the array to initialize each element. + for (idx0 = 0; idx0 < 3; idx0++) { + for (idx1 = 0; idx1 < 3; idx1++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx1 + 3 * idx0] = argInit_real32_T(); + } + } +} + +// +// Arguments : void +// Return Type : boolean_T +// +static boolean_T argInit_boolean_T() +{ + return false; +} + +// +// Arguments : void +// Return Type : float +// +static float argInit_real32_T() +{ + return 0.0F; +} + +// +// Arguments : void +// Return Type : void +// +static void main_QEKF() +{ + float X[10]; + static float P_prev[100]; + float Gyroscope[3]; + float Accelerometer[3]; + float Heading; + boolean_T UseHeadingForCorrection; + float SamplePeriod; + float fv5[9]; + float fv6[9]; + float X_out[10]; + static float P_out[100]; + + // Initialize function 'QEKF' input arguments. + // Initialize function input argument 'X'. + argInit_10x1_real32_T(X); + + // Initialize function input argument 'P_prev'. + argInit_10x10_real32_T(P_prev); + + // Initialize function input argument 'Gyroscope'. + argInit_3x1_real32_T(Gyroscope); + + // Initialize function input argument 'Accelerometer'. + argInit_3x1_real32_T(Accelerometer); + Heading = argInit_real32_T(); + UseHeadingForCorrection = argInit_boolean_T(); + SamplePeriod = argInit_real32_T(); + + // Initialize function input argument 'cov_gyro'. + // Initialize function input argument 'cov_acc'. + // Call the entry-point 'QEKF'. + argInit_3x3_real32_T(fv5); + argInit_3x3_real32_T(fv6); + QEKF(X, P_prev, Gyroscope, Accelerometer, Heading, UseHeadingForCorrection, + SamplePeriod, argInit_boolean_T(), argInit_boolean_T(), argInit_boolean_T + (), argInit_boolean_T(), fv5, fv6, argInit_real32_T(), argInit_real32_T(), + argInit_real32_T(), argInit_real32_T(), argInit_boolean_T(), + argInit_real32_T(), argInit_real32_T(), argInit_real32_T(), + argInit_real32_T(), X_out, P_out); +} + +// +// Arguments : int argc +// const char * const argv[] +// Return Type : int +// +int main(int, const char * const []) +{ + // Initialize the application. + // You do not need to do this more than one time. + QEKF_initialize(); + + // Invoke the entry-point functions. + // You can call entry-point functions multiple times. + main_QEKF(); + + // Terminate the application. + // You do not need to do this more than one time. + QEKF_terminate(); + return 0; +} + +// +// File trailer for main.cpp +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/examples/main.h b/Estimators/QEKF/codegen/lib/QEKF/examples/main.h new file mode 100644 index 0000000..3e42774 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/examples/main.h @@ -0,0 +1,53 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: main.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// + +//*********************************************************************** +// This automatically generated example C main file shows how to call +// entry-point functions that MATLAB Coder generated. You must customize +// this file for your application. Do not modify this file directly. +// Instead, make a copy of this file, modify it, and integrate it into +// your development environment. +// +// This file initializes entry-point function arguments to a default +// size and value before calling the entry-point functions. It does +// not store or use any values returned from the entry-point functions. +// If necessary, it does pre-allocate memory for returned values. +// You can use this file as a starting point for a main function that +// you can deploy in your application. +// +// After you copy the file, and before you deploy it, you must make the +// following changes: +// * For variable-size function arguments, change the example sizes to +// the sizes that your application requires. +// * Change the example values of function arguments to the values that +// your application requires. +// * If the entry-point functions return values, store these values or +// otherwise use them as required by your application. +// +//*********************************************************************** +#ifndef MAIN_H +#define MAIN_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "QEKF_types.h" + +// Function Declarations +extern int main(int argc, const char * const argv[]); + +#endif + +// +// File trailer for main.h +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/eye.cpp b/Estimators/QEKF/codegen/lib/QEKF/eye.cpp new file mode 100644 index 0000000..1a948be --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/eye.cpp @@ -0,0 +1,62 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: eye.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// + +// Include Files +#include +#include "rt_nonfinite.h" +#include "QEKF.h" +#include "eye.h" + +// Function Definitions + +// +// Arguments : double I[16] +// Return Type : void +// +void b_eye(double I[16]) +{ + int k; + memset(&I[0], 0, sizeof(double) << 4); + for (k = 0; k < 4; k++) { + I[k + (k << 2)] = 1.0; + } +} + +// +// Arguments : double I[100] +// Return Type : void +// +void c_eye(double I[100]) +{ + int k; + memset(&I[0], 0, 100U * sizeof(double)); + for (k = 0; k < 10; k++) { + I[k + 10 * k] = 1.0; + } +} + +// +// Arguments : double I[9] +// Return Type : void +// +void eye(double I[9]) +{ + int k; + memset(&I[0], 0, 9U * sizeof(double)); + for (k = 0; k < 3; k++) { + I[k + 3 * k] = 1.0; + } +} + +// +// File trailer for eye.cpp +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/eye.h b/Estimators/QEKF/codegen/lib/QEKF/eye.h new file mode 100644 index 0000000..ba9a99d --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/eye.h @@ -0,0 +1,30 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: eye.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// +#ifndef EYE_H +#define EYE_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "QEKF_types.h" + +// Function Declarations +extern void b_eye(double I[16]); +extern void c_eye(double I[100]); +extern void eye(double I[9]); + +#endif + +// +// File trailer for eye.h +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/html/report.mldatx b/Estimators/QEKF/codegen/lib/QEKF/html/report.mldatx new file mode 100644 index 0000000..1338b02 Binary files /dev/null and b/Estimators/QEKF/codegen/lib/QEKF/html/report.mldatx differ diff --git a/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_api.c b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_api.c new file mode 100644 index 0000000..b465131 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_api.c @@ -0,0 +1,623 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_QEKF_api.c + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +/* Include Files */ +#include "tmwtypes.h" +#include "_coder_QEKF_api.h" +#include "_coder_QEKF_mex.h" + +/* Variable Definitions */ +emlrtCTX emlrtRootTLSGlobal = NULL; +emlrtContext emlrtContextGlobal = { true,/* bFirstTime */ + false, /* bInitialized */ + 131466U, /* fVersionInfo */ + NULL, /* fErrorFunction */ + "QEKF", /* fFunctionName */ + NULL, /* fRTCallStack */ + false, /* bDebugMode */ + { 2045744189U, 2170104910U, 2743257031U, 4284093946U },/* fSigWrd */ + NULL /* fSigMem */ +}; + +/* Function Declarations */ +static void b_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[10]); +static const mxArray *b_emlrt_marshallOut(const real32_T u[100]); +static void c_emlrt_marshallIn(const emlrtStack *sp, const mxArray *P_prev, + const char_T *identifier, real32_T y[100]); +static void d_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[100]); +static void e_emlrt_marshallIn(const emlrtStack *sp, const mxArray *Gyroscope, + const char_T *identifier, real32_T y[3]); +static void emlrt_marshallIn(const emlrtStack *sp, const mxArray *X, const + char_T *identifier, real32_T y[10]); +static const mxArray *emlrt_marshallOut(const real32_T u[10]); +static void f_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[3]); +static real32_T g_emlrt_marshallIn(const emlrtStack *sp, const mxArray *Heading, + const char_T *identifier); +static real32_T h_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId); +static boolean_T i_emlrt_marshallIn(const emlrtStack *sp, const mxArray + *UseHeadingForCorrection, const char_T *identifier); +static boolean_T j_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, + const emlrtMsgIdentifier *parentId); +static void k_emlrt_marshallIn(const emlrtStack *sp, const mxArray *cov_gyro, + const char_T *identifier, real32_T y[9]); +static void l_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[9]); +static void m_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[10]); +static void n_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[100]); +static void o_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[3]); +static real32_T p_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, + const emlrtMsgIdentifier *msgId); +static boolean_T q_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, + const emlrtMsgIdentifier *msgId); +static void r_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[9]); + +/* Function Definitions */ + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * real32_T y[10] + * Return Type : void + */ +static void b_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[10]) +{ + m_emlrt_marshallIn(sp, emlrtAlias(u), parentId, y); + emlrtDestroyArray(&u); +} + +/* + * Arguments : const real32_T u[100] + * Return Type : const mxArray * + */ +static const mxArray *b_emlrt_marshallOut(const real32_T u[100]) +{ + const mxArray *y; + int32_T i5; + const mxArray *m1; + static const int32_T iv1[2] = { 10, 10 }; + + int32_T i; + real32_T *pData; + real32_T fv2[100]; + int32_T b_i; + y = NULL; + for (i5 = 0; i5 < 10; i5++) { + for (i = 0; i < 10; i++) { + fv2[i + 10 * i5] = u[i5 + 10 * i]; + } + } + + m1 = emlrtCreateNumericArray(2, iv1, mxSINGLE_CLASS, mxREAL); + pData = (real32_T *)emlrtMxGetData(m1); + i5 = 0; + for (i = 0; i < 10; i++) { + for (b_i = 0; b_i < 10; b_i++) { + pData[i5] = fv2[b_i + 10 * i]; + i5++; + } + } + + emlrtAssign(&y, m1); + return y; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *P_prev + * const char_T *identifier + * real32_T y[100] + * Return Type : void + */ +static void c_emlrt_marshallIn(const emlrtStack *sp, const mxArray *P_prev, + const char_T *identifier, real32_T y[100]) +{ + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + d_emlrt_marshallIn(sp, emlrtAlias(P_prev), &thisId, y); + emlrtDestroyArray(&P_prev); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * real32_T y[100] + * Return Type : void + */ +static void d_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[100]) +{ + real32_T fv0[100]; + int32_T i0; + int32_T i1; + n_emlrt_marshallIn(sp, emlrtAlias(u), parentId, fv0); + for (i0 = 0; i0 < 10; i0++) { + for (i1 = 0; i1 < 10; i1++) { + y[i1 + 10 * i0] = fv0[i0 + 10 * i1]; + } + } + + emlrtDestroyArray(&u); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *Gyroscope + * const char_T *identifier + * real32_T y[3] + * Return Type : void + */ +static void e_emlrt_marshallIn(const emlrtStack *sp, const mxArray *Gyroscope, + const char_T *identifier, real32_T y[3]) +{ + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + f_emlrt_marshallIn(sp, emlrtAlias(Gyroscope), &thisId, y); + emlrtDestroyArray(&Gyroscope); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *X + * const char_T *identifier + * real32_T y[10] + * Return Type : void + */ +static void emlrt_marshallIn(const emlrtStack *sp, const mxArray *X, const + char_T *identifier, real32_T y[10]) +{ + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + b_emlrt_marshallIn(sp, emlrtAlias(X), &thisId, y); + emlrtDestroyArray(&X); +} + +/* + * Arguments : const real32_T u[10] + * Return Type : const mxArray * + */ +static const mxArray *emlrt_marshallOut(const real32_T u[10]) +{ + const mxArray *y; + const mxArray *m0; + static const int32_T iv0[1] = { 10 }; + + real32_T *pData; + int32_T i4; + int32_T i; + y = NULL; + m0 = emlrtCreateNumericArray(1, iv0, mxSINGLE_CLASS, mxREAL); + pData = (real32_T *)emlrtMxGetData(m0); + i4 = 0; + for (i = 0; i < 10; i++) { + pData[i4] = u[i]; + i4++; + } + + emlrtAssign(&y, m0); + return y; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * real32_T y[3] + * Return Type : void + */ +static void f_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[3]) +{ + o_emlrt_marshallIn(sp, emlrtAlias(u), parentId, y); + emlrtDestroyArray(&u); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *Heading + * const char_T *identifier + * Return Type : real32_T + */ +static real32_T g_emlrt_marshallIn(const emlrtStack *sp, const mxArray *Heading, + const char_T *identifier) +{ + real32_T y; + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + y = h_emlrt_marshallIn(sp, emlrtAlias(Heading), &thisId); + emlrtDestroyArray(&Heading); + return y; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * Return Type : real32_T + */ +static real32_T h_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId) +{ + real32_T y; + y = p_emlrt_marshallIn(sp, emlrtAlias(u), parentId); + emlrtDestroyArray(&u); + return y; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *UseHeadingForCorrection + * const char_T *identifier + * Return Type : boolean_T + */ +static boolean_T i_emlrt_marshallIn(const emlrtStack *sp, const mxArray + *UseHeadingForCorrection, const char_T *identifier) +{ + boolean_T y; + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + y = j_emlrt_marshallIn(sp, emlrtAlias(UseHeadingForCorrection), &thisId); + emlrtDestroyArray(&UseHeadingForCorrection); + return y; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * Return Type : boolean_T + */ +static boolean_T j_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, + const emlrtMsgIdentifier *parentId) +{ + boolean_T y; + y = q_emlrt_marshallIn(sp, emlrtAlias(u), parentId); + emlrtDestroyArray(&u); + return y; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *cov_gyro + * const char_T *identifier + * real32_T y[9] + * Return Type : void + */ +static void k_emlrt_marshallIn(const emlrtStack *sp, const mxArray *cov_gyro, + const char_T *identifier, real32_T y[9]) +{ + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + l_emlrt_marshallIn(sp, emlrtAlias(cov_gyro), &thisId, y); + emlrtDestroyArray(&cov_gyro); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * real32_T y[9] + * Return Type : void + */ +static void l_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[9]) +{ + real32_T fv1[9]; + int32_T i2; + int32_T i3; + r_emlrt_marshallIn(sp, emlrtAlias(u), parentId, fv1); + for (i2 = 0; i2 < 3; i2++) { + for (i3 = 0; i3 < 3; i3++) { + y[i3 + 3 * i2] = fv1[i2 + 3 * i3]; + } + } + + emlrtDestroyArray(&u); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * real32_T ret[10] + * Return Type : void + */ +static void m_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[10]) +{ + static const int32_T dims[1] = { 10 }; + + int32_T i6; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 1U, *(int32_T (*)[1])&dims[0]); + for (i6 = 0; i6 < 10; i6++) { + ret[i6] = (*(real32_T (*)[10])emlrtMxGetData(src))[i6]; + } + + emlrtDestroyArray(&src); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * real32_T ret[100] + * Return Type : void + */ +static void n_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[100]) +{ + static const int32_T dims[2] = { 10, 10 }; + + int32_T i7; + int32_T i8; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 2U, *(int32_T (*)[2])&dims[0]); + for (i7 = 0; i7 < 10; i7++) { + for (i8 = 0; i8 < 10; i8++) { + ret[i8 + 10 * i7] = (*(real32_T (*)[100])emlrtMxGetData(src))[i8 + 10 * i7]; + } + } + + emlrtDestroyArray(&src); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * real32_T ret[3] + * Return Type : void + */ +static void o_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[3]) +{ + static const int32_T dims[1] = { 3 }; + + int32_T i9; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 1U, *(int32_T (*)[1])&dims[0]); + for (i9 = 0; i9 < 3; i9++) { + ret[i9] = (*(real32_T (*)[3])emlrtMxGetData(src))[i9]; + } + + emlrtDestroyArray(&src); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * Return Type : real32_T + */ +static real32_T p_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, + const emlrtMsgIdentifier *msgId) +{ + real32_T ret; + static const int32_T dims = 0; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 0U, (int32_T *)&dims); + ret = *(real32_T *)emlrtMxGetData(src); + emlrtDestroyArray(&src); + return ret; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * Return Type : boolean_T + */ +static boolean_T q_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, + const emlrtMsgIdentifier *msgId) +{ + boolean_T ret; + static const int32_T dims = 0; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "logical", + false, 0U, (int32_T *)&dims); + ret = *emlrtMxGetLogicals(src); + emlrtDestroyArray(&src); + return ret; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * real32_T ret[9] + * Return Type : void + */ +static void r_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[9]) +{ + static const int32_T dims[2] = { 3, 3 }; + + int32_T i10; + int32_T i11; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 2U, *(int32_T (*)[2])&dims[0]); + for (i10 = 0; i10 < 3; i10++) { + for (i11 = 0; i11 < 3; i11++) { + ret[i11 + 3 * i10] = (*(real32_T (*)[9])emlrtMxGetData(src))[i11 + 3 * i10]; + } + } + + emlrtDestroyArray(&src); +} + +/* + * Arguments : const mxArray * const prhs[22] + * int32_T nlhs + * const mxArray *plhs[2] + * Return Type : void + */ +void QEKF_api(const mxArray * const prhs[22], int32_T nlhs, const mxArray *plhs + [2]) +{ + real32_T X[10]; + real32_T P_prev[100]; + real32_T Gyroscope[3]; + real32_T Accelerometer[3]; + real32_T Heading; + boolean_T UseHeadingForCorrection; + real32_T SamplePeriod; + boolean_T SensorDriven; + boolean_T BiasEstimationEnabled; + boolean_T YawBiasEstimationEnabled; + boolean_T NormalizeAccelerometer; + real32_T cov_gyro[9]; + real32_T cov_acc[9]; + real32_T GyroscopeTrustFactor; + real32_T sigma2_omega; + real32_T sigma2_heading; + real32_T sigma2_bias; + boolean_T AccelerometerVibrationDetectionEnabled; + real32_T AccelerometerVibrationNormLPFtau; + real32_T AccelerometerVibrationCovarianceVaryFactor; + real32_T MaxVaryFactor; + real32_T g; + real32_T X_out[10]; + real32_T P_out[100]; + emlrtStack st = { NULL, /* site */ + NULL, /* tls */ + NULL /* prev */ + }; + + st.tls = emlrtRootTLSGlobal; + + /* Marshall function inputs */ + emlrt_marshallIn(&st, emlrtAliasP(prhs[0]), "X", X); + c_emlrt_marshallIn(&st, emlrtAliasP(prhs[1]), "P_prev", P_prev); + e_emlrt_marshallIn(&st, emlrtAliasP(prhs[2]), "Gyroscope", Gyroscope); + e_emlrt_marshallIn(&st, emlrtAliasP(prhs[3]), "Accelerometer", Accelerometer); + Heading = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[4]), "Heading"); + UseHeadingForCorrection = i_emlrt_marshallIn(&st, emlrtAliasP(prhs[5]), + "UseHeadingForCorrection"); + SamplePeriod = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[6]), "SamplePeriod"); + SensorDriven = i_emlrt_marshallIn(&st, emlrtAliasP(prhs[7]), "SensorDriven"); + BiasEstimationEnabled = i_emlrt_marshallIn(&st, emlrtAliasP(prhs[8]), + "BiasEstimationEnabled"); + YawBiasEstimationEnabled = i_emlrt_marshallIn(&st, emlrtAliasP(prhs[9]), + "YawBiasEstimationEnabled"); + NormalizeAccelerometer = i_emlrt_marshallIn(&st, emlrtAliasP(prhs[10]), + "NormalizeAccelerometer"); + k_emlrt_marshallIn(&st, emlrtAliasP(prhs[11]), "cov_gyro", cov_gyro); + k_emlrt_marshallIn(&st, emlrtAliasP(prhs[12]), "cov_acc", cov_acc); + GyroscopeTrustFactor = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[13]), + "GyroscopeTrustFactor"); + sigma2_omega = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[14]), "sigma2_omega"); + sigma2_heading = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[15]), + "sigma2_heading"); + sigma2_bias = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[16]), "sigma2_bias"); + AccelerometerVibrationDetectionEnabled = i_emlrt_marshallIn(&st, emlrtAliasP + (prhs[17]), "AccelerometerVibrationDetectionEnabled"); + AccelerometerVibrationNormLPFtau = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[18]), + "AccelerometerVibrationNormLPFtau"); + AccelerometerVibrationCovarianceVaryFactor = g_emlrt_marshallIn(&st, + emlrtAliasP(prhs[19]), "AccelerometerVibrationCovarianceVaryFactor"); + MaxVaryFactor = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[20]), "MaxVaryFactor"); + g = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[21]), "g"); + + /* Invoke the target function */ + QEKF(X, P_prev, Gyroscope, Accelerometer, Heading, UseHeadingForCorrection, + SamplePeriod, SensorDriven, BiasEstimationEnabled, + YawBiasEstimationEnabled, NormalizeAccelerometer, cov_gyro, cov_acc, + GyroscopeTrustFactor, sigma2_omega, sigma2_heading, sigma2_bias, + AccelerometerVibrationDetectionEnabled, AccelerometerVibrationNormLPFtau, + AccelerometerVibrationCovarianceVaryFactor, MaxVaryFactor, g, X_out, + P_out); + + /* Marshall function outputs */ + plhs[0] = emlrt_marshallOut(X_out); + if (nlhs > 1) { + plhs[1] = b_emlrt_marshallOut(P_out); + } +} + +/* + * Arguments : void + * Return Type : void + */ +void QEKF_atexit(void) +{ + emlrtStack st = { NULL, /* site */ + NULL, /* tls */ + NULL /* prev */ + }; + + mexFunctionCreateRootTLS(); + st.tls = emlrtRootTLSGlobal; + emlrtEnterRtStackR2012b(&st); + emlrtLeaveRtStackR2012b(&st); + emlrtDestroyRootTLS(&emlrtRootTLSGlobal); + QEKF_xil_terminate(); +} + +/* + * Arguments : void + * Return Type : void + */ +void QEKF_initialize(void) +{ + emlrtStack st = { NULL, /* site */ + NULL, /* tls */ + NULL /* prev */ + }; + + mexFunctionCreateRootTLS(); + st.tls = emlrtRootTLSGlobal; + emlrtClearAllocCountR2012b(&st, false, 0U, 0); + emlrtEnterRtStackR2012b(&st); + emlrtFirstTimeR2012b(emlrtRootTLSGlobal); +} + +/* + * Arguments : void + * Return Type : void + */ +void QEKF_terminate(void) +{ + emlrtStack st = { NULL, /* site */ + NULL, /* tls */ + NULL /* prev */ + }; + + st.tls = emlrtRootTLSGlobal; + emlrtLeaveRtStackR2012b(&st); + emlrtDestroyRootTLS(&emlrtRootTLSGlobal); +} + +/* + * File trailer for _coder_QEKF_api.c + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_api.h b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_api.h new file mode 100644 index 0000000..42709c0 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_api.h @@ -0,0 +1,53 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_QEKF_api.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +#ifndef _CODER_QEKF_API_H +#define _CODER_QEKF_API_H + +/* Include Files */ +#include "tmwtypes.h" +#include "mex.h" +#include "emlrt.h" +#include +#include +#include "_coder_QEKF_api.h" + +/* Variable Declarations */ +extern emlrtCTX emlrtRootTLSGlobal; +extern emlrtContext emlrtContextGlobal; + +/* Function Declarations */ +extern void QEKF(real32_T X[10], real32_T P_prev[100], real32_T Gyroscope[3], + real32_T Accelerometer[3], real32_T Heading, boolean_T + UseHeadingForCorrection, real32_T SamplePeriod, boolean_T + SensorDriven, boolean_T BiasEstimationEnabled, boolean_T + YawBiasEstimationEnabled, boolean_T NormalizeAccelerometer, + real32_T cov_gyro[9], real32_T cov_acc[9], real32_T + GyroscopeTrustFactor, real32_T sigma2_omega, real32_T + sigma2_heading, real32_T sigma2_bias, boolean_T + AccelerometerVibrationDetectionEnabled, real32_T + AccelerometerVibrationNormLPFtau, real32_T + AccelerometerVibrationCovarianceVaryFactor, real32_T + MaxVaryFactor, real32_T g, real32_T X_out[10], real32_T P_out + [100]); +extern void QEKF_api(const mxArray * const prhs[22], int32_T nlhs, const mxArray + *plhs[2]); +extern void QEKF_atexit(void); +extern void QEKF_initialize(void); +extern void QEKF_terminate(void); +extern void QEKF_xil_terminate(void); + +#endif + +/* + * File trailer for _coder_QEKF_api.h + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_info.c b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_info.c new file mode 100644 index 0000000..ba374e0 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_info.c @@ -0,0 +1,287 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_QEKF_info.c + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +/* Include Files */ +#include "_coder_QEKF_info.h" + +/* Function Definitions */ + +/* + * Arguments : void + * Return Type : const mxArray * + */ +const mxArray *emlrtMexFcnResolvedFunctionsInfo() +{ + const mxArray *nameCaptureInfo; + const char * data[109] = { + "789ced7d6b8c2bc9751ef558eb2e14c5b4d6b256efbd92b09025ebcef031c3998577c53787c3e16bc82167b8ab9d69924db239fd20bb9b1c729c00349008fa61" + "1b129c00096018f78f015b819db503449b5830a8d83f8cfc52003901ec3f0bc78215c308040412202081c26677cf9dee65dde19d2e56b39be700bb35e461d757", + "7deea973aa4e559df2bc279d7d8fc7e3f9a7b3ff94f2eb1f7acea3d0873c2a79b5f2bd1e2399f9efd1caff61faacd3739ef71b9ed3f9bfa3950d8197e991ac7e" + "60199ece0db83a2dce3ef01447df54d3143886a778b93ceed11e919604764837e79c16c3d26586a38f845b1f0e98d9072e798b75f34161297fc73a74e3b234e0", + "3c62477ad25cf6f607cf2df93c46bcfffb97944f18211faf89ff7ae2abb157b64e245a94b6ca1d81a3a4adb8d01870342f4b5b99419ba5bf9c8d948f22d1ad84" + "24331c250bb31f161399e4fc7f8f38bdbd1788f6fcdc92ed35973a3def7970ebd3af84efc25b563eef377df6dcfc4ee53419aaad94b8f07ecef4f9099eca690a", + "833a4b3fc1fb7d8b7807483c23fff5f4d1e95c050aa2d01629ee254557a52deddffcd8bfeddba3b6644160ebc2688be6d82d96a96fcdd480a5ea5b343bfb634b" + "9114513df85fbfc7fff70859bdbb5b0fdc823742d4b7acde7d0481e735f1fd693fd5decb1f2619a6715d8a1d24c460a776ab1d853b70ee6a8707f19954fdd07f", + "8d64d4b757b1d9f107a6cf9e9bdfa91c469abda3c88cecb2e36f59c43b42e219f9d6f54097d4234ea98f9c3e84bffbc514d8f375b5e7bf84c0f39af8b9c66535" + "2d261bada47c1a68f88a3bd1b36c3405f67c23ecf9a41cc6a56f1f45e0794dfc86d0a4c547cc6c3227f214fb486a502c2526dab68dd7addaf982e9b3c7f43b9d", + "ffccfaa1fcf7a5b9b4b6bea48b6b4b1717493d99fcd1cb6f829d77ba9ddf3e6efb86f2e1be20b63b522949b5a3c9f430ee1e3b0ffd7871fb8df63e7b63ef7b88" + "fa969597394ee731fd4ee733123fe0689169c84a44cebeb88c55fdc823f18cfc7b8d035acc886ef684997a6c19e4351bd213d48f3fac7eecafc0ceaf088f949d", + "4f052ec7a57e9b6163e5d8e5713fda2e5f9e24c0ce6fac9d47e1e18bd3b494d5858e53edfa2112cfc8b76ed75539a9511aa27608ecfa0af148c5dd877d3ad11b" + "36a4fdd3ab802014a4922f70e6a6b8fbdb88e79795e329a27eaf898fdbae3f6c31a224b718a2faf7ce05b1f554c56e29a553d75393483c231f877d576c3b413d", + "78fbc77f0d767d5dedfab2e3f5d2b8c7703bfde3aa8fa93783e95ab190f5b9292e03fdd748467ddb764d3ce62e3fd2e850ca662d88c7204a9d60dc4e060fe231" + "78ea77ecb8fd9a16056940362e337d83d8b8bd356059a584b80c7add5dea51626bc06f29b2221f9799fc31d877e7db7779d7cf1dc7aba94e2d205c0fe52c97cc", + "fa6417d977d83f6324639cbd7263cf2788fa9695d3a711785e137ff6eae7337776de124456107ae7c290165bac7075de50ce2958b7f76642b547271d6f7a4f3c" + "bdfe0bd367339eceb7a427eab0e029f223a93fd3effce039d857e374fb2f6512a344fcfaba7038f207b3feed5831e2db4eb8c7feff10f1fcb272fc1aa27eaf89", + "bfea7efdf0e93f38efd06c4f394e46565f53b6c5f16ddaefc9f04d7a94e665880f22da0df17d327810dfc7533ff807637bf1ea6b1a9b7fb82b7e337b758e1a39" + "773d8044bc483b6f359714f978d1f4ed8f3e0ff305a7fb03b1bfddeb325230c2ed16427ca17e1ac854a428f803f0078bdfdb186f22eb0f181efcc172fe80e16d", + "f0077f0afec0f9fea04d9d85f2816aae1faa47e4d80e950fd2c1a28bcedf3e463cbfaef9700edeb3b83dcbead9a710ed7d5e2b7f68f836ae353fa79549b59c68" + "65f875edb3c67f8751cb6949e3d7b4cf5ae9696be59b1a5f2ba717daf35ae9e96a655d2d1f6ba5a7a17dd64a4f537b9ed6be6fa9e545476b97565e68ed9ae8ed", + "d3ea7fa78bed7cf12f983e7b4cbfd3f98adbe666ffc0b474e3a2e764d7ba88d5fd11a53bf074be153f26f4a4ad77c94d736804d7c333bff06be0cf9ceecf0e4b" + "cd606694cbd5cb7225d866c7995a3e74e8a278d75f229e5f568e14a27eaf89bf82fefcb021709cc0ab13183d7ddd5dfb4f97d5c7f79a3eeba4fb3da37dcc87d5", + "b2806dfdfc33087caf896f5e8f90a2038695d37c4edde5896dbef301647b540e2bb49906c53a741f9d596c44f3517c1bd6459cef27f2c37e2cd23a18ee958bc1" + "7123c1f1fe648d3d708f9f788c787e5de73d53447b96d5b37f8268af6eff2f6e7f39d1e721e75aa9cd773cfaf7a6f9cb3bda3c449fbf98e723fabce3d67c43c7", + "bbaffcdf87781fafc6a1c76ad259a79e8b4b21f18c7cebf1b199a4c89e9b803ca32bc423e51f762a815450cc0c2bf9fcceb1b49f2b15b33bb28be262d07f8d64" + "d4b7d7b0c58d5e44e0794d7cd3bc80a7c436c3373a974edd4ffbc61d783aff99f443ea5022dddc526703da9cc03c35b8911ce97ca493175e39fe3ed8fd15e191", + "b2fbe3d1557a9f4ab6da3bdb8d7c4228759383d0958bf6d34e11cf3bb55fa3f462593d448db3f579c38b866f3f1b56cb97b512dfbd02cf21dae1d5382d561094" + "b504a79ea74b23f18c7c6be385f9693a4552fa7238497bf5dd9397212ee474fbbfd70fe5cb7d36be7319ed948ba992dc8fc71b49f7d87fe8c78bdb6fd4bb975c", + "bb2ef0ac761fd60516973ac1ba00193c5817c0533fd8ffc5ed37eadde7b1d9ff4f20f0bc26fec27b091ee9a37ee7ee1f3ab9034fe7e3b1ff5f52e5764b7708da" + "ff6f413e0de7dbff7a67b770cab372b3247089017f10a827a2872e8afb83fd5fdc7e94fdff06a2be65e5f5cb083caf896fb2ff54afc78e4b7363961cf00d9911", + "f8345f60a9867ed5e85dfba6966ddfcfdfd13e9ddfd25a71dea1f8e66c82806b3f5afd0e7c9d8fc73fa0c56ac3f989fa07bbb04eec747f215047dd3a45a562cd" + "c05e573af5d3d95a6f0cfb4da17fcf09dffac0ddf72028d9e424daa9eb0339249e918f23df9e2e2bb2f934c20f5e857bcc1c6fefafaa6929c95e5692fe965862", + "87ddfc217f5d8c81bd077bafd023d7c58deeeb0f206eb4b8d409e24664f0206e84a77ed82f6a24635e8c03bbe345f3337511be59a2e48148c97462d4a3669f98" + "6b73bce8c262fb9ed50f386b3c8116a30de3897f9beac17cc1e97e61581fb484a43fb7dd157274a013c99d0a33d7e01ebf00fd7bf17b2da78f5fc4161f82fb18", + "5482fb1896c5536953f0e03e063cf583bd5ffc5ecbe9e393f58009a23ed857ba1c1eec2b5d159e4a9b8207fb4af1d40f7e61f17b2da78f811bbff075447dcbca" + "f12102cf6be29bf71b49122dca158a65e65288886ded7776f987a945bc1a12cfc8c7b40ef52ef1d9b09fe8bdb12b881739dd4ff8fd97d7fbf16434dd3e1a956a", + "d707f583fd63d945f1a229e279e8d74632ae2f84611fd19278b08f08379e4a9b8207fb88f0d43f453c0f76de48463b1f81f890f61b880f2d2e7582f810193c88" + "0fe1a9df6a7c88367df6987ea7f357e70f1e3292c29f55ca7a08ebe3f48c589e21e5255b1ee7ae1393399f38bf774791940de71327702fe70af188e5171d65d9", + "e4e5b0d94def9e8e0e9ba23fc8977621cfd006f56385f0e59980f13e0acfc887f1feb3e2a9b4297830dec753bf4bc6fb7561c037250fe9f17e119b5f40dd07e7" + "35f1dfe517d2ca5c871663ec4c361e7cf38167bd87d3aa5fa820f18c7c5c7ee1b6d808af0bfcc62fc17d9c8ef70b49fea8108d5473d70c37ca777b4d2e2fe7c6", + "2e5a1700bfb0f8bd96f30b27d8e240778dcfd55be6edb3fb56e78f64ef5fa64636ecf7791bee5f76bebd17fbdbbd2e230523dc6e21c417eaa7814c458a82bd07" + "7baf5085a8bd6778b0f7cbd97b86b7c1deff29d87be7dbfb367516ca07aab97ea81e91633b543e48078b2eca1331453c0ffb7e8c64dcf77304767e493cb0f3b8", + "f154da143cb0f378ea9f229e073b6f24a39dcf41fc66493c88dfe0c6536953f0207e83a77ec8fb66a455c567e05c964a702e6b593c9536050fce65e1a91fecb9" + "918ce3f32ab67d379f46e0794dfcd9ab9fcf46a0e72d416405a1772e0c69b1c50a57e7f31415d6c7ef6642b547275cf3bb0bd367339eceb7a427ea74ef29f223", + "b92f73fa9d1f3c07f6dfe9f65fca244689f8f575e170e40f66fddbb162c4b7eda2fc0b56d7632944fd5e13df4abf167ad2bc5f7332c3d1d27987667bb4f8887b" + "d810384ee0d59e2d99deab8768f7b2faf85ec47be9f7c01bed643eac96858dddbf2f317c1bf6efdfc7aec1fefd55e2c1fe7d3cf5839f58fc5ecfe6270a61b5cc", + "6fac9f80735e8bdf03fc84bd78e027f0d4ff18f1fcb2720c23eaf79af8afcf3bf389448bd256b9237094b415171ab35ec9cbd25666301b8b7e59ebdb0969e60e" + "285998fdb098c824e7ff7bd26f517e6d593dfb90e9b34ebafdef18becda9cd0f17d572a27dbeb854cb69457bbd37b5df69e5b4a595176af98e5e6adf4f3ada73", + "5d62eb0f9cd864864c93d8fa83797e6175fd81c47ab2321ed1e5f4481b6810f417bf35fe5d57c79f6cbd1fc1aabff80802cf6be28fce6ab5443bc288fde0904b" + "ec94cf22c52ee5718fbf807ebcb8fd46bdfbd15770d9d90f20f0bc1a477b4ddbe24656f581449e10451f7475e0b4fa08daf57ff8c61baeb6ebb6e2919a0704aa", + "432151acfa0a6c82921ac7b2988f676817ed13727a3f261b17fa6c582d5fded8b810ac1f2c7e8f25fc01c48556880771213cf53bdd1f5c20da8757efbe88cdfe" + "bf84c0f39af80bee8ba747bd98c0f52899a9b3b6c577a616f1ce9078463e1efbff2eb1d9a03fe137ffe205981738dd0f749a3db1767a9d68b443f1a3d03830a0", + "22c53d98176c981ff8d5302ebdfb3002cf6be29bfcc0ecedd5ef9d1a07ca20f18c7c3cf67f262e4d5308dafb4ffeb409f6dee9f6be1daad1856267d8eca7bbb1" + "72af9ac8f5aabc8bf6973addde431c08f607411c68119e4a9b820771203cf53f463cbfaefb8350ffeecbead9fb10edd5edff8b866ff36af3275a7971896d3fcf", + "5df91a78419cbb3ea78ef7498c136685729c5811950df3c2e9e7fe150de37da7dbf7e6ee20b87bdaa4a952abdc66d8506e903da8bbe83c31f4e3c5ed37eaddab" + "d8ecfaddf777f114efb1efdcb013f441bff7672629b8bfcb6578a4f669f623833c171c9f06b78f45ff20e6a3f7875ccee31ebbbeb6f370945d7f383fee956e69", + "3371a2fa37e1217ee35973bd81f8cd9c36050fe23778ea5fdbfebc8e7ee031efb871fe7ded3c8cf317b71fc6f964f0609c8fa77ee8c78bdb0ff7f4c2f8fe69ef" + "01e37b7bf1607c8fa77eabfdf91851bfd7c4c737be6fd37c4f5fbd24ac7fe10e363ff031049ed7c437f9813a4b498f46bcc8f935be53cf6b959078463e1e3ff0", + "25456e5b73b911dfb739f9d9d75e8375dc75f503cbce034a7cbc9db8ec9d1493fbd9d3504f6ec6d252d9037e60f3fab3427e6c7ee0e3083caf89bfc80f303ccb" + "f0da059876adf75ad59bf21d783a1fa7dea872531587a0deec3ef8f5ef831f58111ea9f940b44e45a4682e5818b27b455e96f7aa97fdbe8beef5023fb0b8d4c9", + "a87fbb76c785e67e6020d1e722dd52feb6cd0f4cef89a7d74ff6fcaeaa37b7e466475c11fcc10af188ddff15c9064b91562cdf48485297aaa43bfb71ce45fb3b" + "c11f2c2e7532ea5f089b3ff82402cf6be29bfc816ece6e42444e8d0f559078463e26bdd1e4f6447548eacd7ffd32c4879cee0722fe2e53ec14864c72eccba4d9", + "fd71b678b20ff3828deccf9e4918db3e20d47932afc6a17bea0d07a4ce8d99eff975d07d723d89e87d7293bffbedcf835d77ba5d6fb2f2613229f5f6533213f5" + "3722f1645daac6dd63d7a1ff1ac9a86ff8f2327c0281e735f1dfb5ef27c90a941c9bf9327bedbcb3e2fcb7c5463ace3ff9834fc1bdee8eb7fb57ad00c38d13fe", + "ed03f1e8b2e48b0453a576de4579d9c0ee1bc9a86fdbd8c6ef77e5df57ee2f53c6f09b60d70bd614a12732434aa6b7349191b6ebe17f6fe7fd236ec72365d7f9" + "5a772f231e5ea652470151a0af853da1b0e3a2f13cf4e7c5a54eabdad77fcffd9c2d65989a9d7dc76a7ca7fa8195ed035e38be7f22367de1969cde4cbf057ec0", + "f97e60b8170886e8c34cbe15df976b21eaf828c670b0afffa63e88d71bebc376df164db11cc3db66e7dfb288b7b23ccbef1a1e689222be8e3f7de14588df38de" + "be37e45c79d71f4bf77389c3fa3030e89f1ef9fc90677983fab142afdcd8f51ea2be65e5f541049ed7c4572633ba8d57c8a9e3f96739a78b711e78a32c10d771", + "071e317bbfbbc3b1d2119f190df7bad4612599de29c0787e13fbb3422f415cc783476f20aeb32a3c9536050fe23a78ea87b8cee252a755c575eeb2bb525f9495" + "d2aefdf656d7f30f907846beb5f9a092c6439114d1f1fd774f5e06bbee74bb1e4c255a9deddd74379f08a4e54291e787b1b88bec3af45f23ad6a1fe63dcf5531", + "525ce028864f88a260185f5b8d2bfdfc1dedd1f933c9376446e0cf3b14df9c197e5ce381933bf0753eae7d99b7c4381fe213dc97f99fffb10e717da7fb0136cb" + "35b94b91cef66255794f18345b6703c645fb32a13f2f2e7542e55f40e12d2bb707a6cf9e9bdfa91c46927a9428d9763ffa5b16f172483c23dfcaf84091903242", + "d0654576be177ef0ea9b60df9d6edfafaa6929c95e5692fe96586287ddfc217f5d847c0a1b6adf9fdc9bf575447dcbcaed73083caf896f1af753438169464491" + "1a27594a96697e6696e7bfb3ea07cc846a974e3adef49e787afd6fdc81a7f3f1e8cf22f111dfe73bf9098cfb1def17ae0f0f02b193de2957ace79b91d3d64176", + "c4e55d14ffb1ea17aa88fabd26bed2af63d6d7e98cd108e2e7b39eecdb27b6bfc0e817e669e5952d3ea4f68d0ef84b5eb8e2b1cd07f2483c231f8fbe68e222ab" + "27df7bae0376dfe9763f941f9cf59860f3a09df3278ab1503f12bcbe8678cfc6dbfd09a2be65e5f669049ed7c437d9fd38235175968e0b728112691bedbfb3f4", + "c62436b27af393ff0df7b038de0f8c32dd56ebaa9429ee0c0f1bfd46743bc457072edacf3f453cbfac1c6b88fabd263e9efe4cf1023fe6848174ae7b04f2fbf5" + "14faa4ddfec0bc2c0bfe60edc711e18fef0e605ee0747f104bfa7ad9583294e4afaf8ecbd1dde0ce5ee328e91e7f00eb048b4b9d8cfa17c5b64ef01081e735f1", + "cdeb04ef7288daefd6659fd0d422fee68d2fc2ff1cfcc4fafa8965efed2aee9e1e5c1d974afde460bbee2f9413c3603ee3013fb1917e62f2c44f58d5bf171178" + "5e13dfe427861196cd8b117ebc2e7ec1aafe14efc0d7f978f4e7467c76dce7fe75d83fea7c7f70cd0ffad5e42893ed44aabe60a91e1b67527e0ff883cdebcf0a", + "c1fe229d607fd1e2f759428f607fd10af1607f119efae17c99918cfaf6056cf3825f44e0794d7c931fa0cde7ca2e2cb6639df3052dad1fd2ac9174734bb5fa9a" + "ed37bb005a9d4a129c07fc59e60cecbdd3edfd882f5e5d677b5c34d58f27a5bdd8617f286cbb280f34d87b2319f5ed11b675e37bdedf2e352896121fe95923ec", + "1bef3beb5e972fa972d374e6f67b5c20da89313fd0b7ffe7dfc33ea275b5fbcbc67ff60647a5ed4a201f11d381047f7a78bccb1fef7ac0ee6f9addff06a2be65" + "e5f4cb083caf896f8ef7f47aecb8343762492d229fe60b2cd5a0b5dfafcbbac05f5ac4afdf81aff331c5819062b5218f5cfd835d981facab9f58767e205047dd", + "3a45a562cdc05e573af5d3d95a6feca2f901f4efc5efb59c3efe0ae49d58122f87c433f221efc4b2782a6d0a1ee49dc0533fd8fbc5efb5a9f1a2fbfa0388172d" + "2e75827811193c8817e1a91ff2492f2e7542ed1f9d20ea5bf1bd020cdfa44759861f481a9f941f680a833a4e3f40f65e812762b361fc50f9b0fc7df0032bc223", + "353fe8723bd144745cacd1dc909583742e19d81f401e0af00388fa48f88132c3d1663f60b53f7cf48ef6e8fc45ed49f3b293fdc35c9c369c3303ffb0423c52fe" + "813b6b1c05760ba5d451ef30d48c67fb43a1da81f3c89be91f6e9d47b6e99cd9dca015d881e436bf40f67cca8d18ed389f027e618578a4fcc241e9e0f4b414c8", + "57e51495bfeca633d9f6ce550afcc266fa8518f17c45ca259cb3d73f6f09222b08bd7361488b2d56b83a6f74e8c6a573cf995d983e9bf174bea57d6aaa1a3d45" + "7e24f7af4dbff383e7609dd9e9fe40ca244689f8f575e170e40f66fddbb162c4b7eda23c76e00f16973a19e3484ffc010a6f59b9bd0f81e7d538545d8d14d9b5", + "6e6c755f720a8967e45bdf973c9314d17c74d329dc4fe97cbbdeafd79bad60af9e2f174a6251f0a552ec51d045eb03d07f8db4aafbe63f83c0f39af8efba9f72" + "9e623fcde7061c2d320dc7ee0f5ac939e2a7e4a3328a8de839e26fff18f2523bdeeee787fd58a47530dc2b1783e34682e3fdc91a0b792336c4ee6fdbbd0eaced", + "0bd547f7ce3d475cba034fe7e3dd17aaeaccedf7b840b413e378ff9b7ff30ad87da7dbfdee4eaad86e162be9fd64ceb75fa91777daad828bce0b80dd3712caee" + "5bcd1bf759049ed7c4bffb1cb1fabb75393f3cb588fffa1df83a7f65e74dc8fb85bf85fca2cef70b998aef52085eb7a8e6d555f5d81fccee274fce5c14df9f22", + "9e877e6d24a31eee60f31790675425c833ba72bc396d0a1ee419c553ff14f13cf8072319f5f0c07571241d9ff4fa01c4915685a7d2a6e0411c094ffd8f11cf2f" + "2bc730a27eaf89fffabc339f48b4286d953b0247495b71a131e0685e96b63283998df9b2d6b71392cc70942ccc7e584c6492f3ff11ce433729c0be9f25f1dc1b", + "47847d3fabc4837d3f78ea7f8c781eec3719fb4d8fd4483ed8efbbecf74c5264d781be0bf6dbf9f67bbc5d3dcd3257f9c3dd2381b94afbf78227639f8bce6741" + "ff359251df3ebb267117ddcac3fe9d678bbba83a73fb3d2e10edc4386eff17ffefbf80dd77badd4fb70e5bbe832379af4af9fa95466e9b3a3de8b828bf33d87d", + "23a1ecfe3710f5c13d00ea33903776f17b2de127e01e8015e2c13d0078ea87febdf8bd96d3477c79a1d765fea0e36fc6ba2dcc1fdc8607f3073cf53f463cbfd1" + "71ff29b9b83fc7a81bfaedb2e76f59c44b23f18c7c2bf3c7262553ca0c72262b1bf2b3857ff47c03ecb8d3edf87e669c3e9585f8beaf7be51fef64f72a9952db", + "45761cfaf1e2f6a3f237d894af93a7c436c3373a97b01fff19ef7fbf911ce9fdf893175e3986fc9c4eb7ffe3d1557a9f4ab6da3bdb8d7c4228759383d0958bce" + "6b81fd5fdc7ea3de7d119bfdbf679ee4d9abe7c52c3572ec78bf60faec31fd4ee7e389dfe8e2229aafe773fff737219eef747b2fb639f9486e64fa1d263f6e5e", + "73f140f3b0e8a2f357d08f17b77f55f61ec6fb8bf160bc6f154fa54dc183f13e9efaadaee7b610f57b4d7cdcf6ffe160d6b1c77931ca28659c917a94dce890d6" + "c757c02fdc130ffcc266d869d278e017f0d40f7e61f17b2da58f9308b67d3e9f42e0794d7c935fa8cf5f3fab47899cbbcf87ecfd0e46b191bddf6192ff3f32c4", + "8b9cee17eab554adbe33ac5212ede7c689203fd8a1ae5d949f01faf3e25227d47e4f981f3c1b1ecc0f36c32e93c683f9019efaadce0fda88fabd26fe6afcc043" + "f58bfc40ee0de4f2b84713d6c713bbd78fd5fdec8936ac1f2fa53fbab888ae1fffd1cb6fc27cc0e97e60fbb8ed1bca87fb82d8ee48a524d58e26d343d82fba41", + "fd784693ec8dbdef21ea5b565e1f327df6987ea7f3198957afa89215efe6dc730179249e917faffd642d6644377bc24c3db60cf2227bbefc0fab1f8373014eb7" + "f3a9c0e5b8d46f336cac1cbb3cee47dbe5cb9304d8f98db5f328bc65e5f5c0f4d973f33b95c3482de5745bc7a976fd108967e45bb7ebaa9c6c38ef0d767d8578", + "56edfa4710785e137fd8a713bd6143da3fbd0a0842412af90267358f7becbab3ee5f7d62d71fb61851925b0cd9f3bc17d8d6753f8dc0f39af833319ccf5effbc" + "2588ac20f4ce85212db658e1eabcd1a11b97f6e57f9bde134fafffc2f4d98ca7f32de58b52d5e829f223393e987ee707cf413c675dfdc1b2e37c29931825e2d7", + "d785c3913f98f56fc78a11dfb68be2fae00f16973a19f4ef9d0b6ce3fdf79b3e7b6e7ea7729471ac52da35deb79a1f3089c433f2718cf709aff3bf0df7b23bdf" + "ae97c63d86dbe91f577d4cbd194cd78a85accf4d717ac7daf56b5a14a4c1cd380d153f5f56ffde8b788fe74dbf9f53f8abe179397d83989d6f0d58562921ae83", + "1ed74b3d4a54ce7f2bb2221fd799fc31c4759c6fefe55d3f771cafa63ab580703d94b35c32eb93c1dedfd4b759fb348f08c6ed15eb25d14eb5ef39249e918fc3" + "beebb222ab0fe107afc2be1bc7dbf7ab6a5a4ab29795a4bf2596d861377fc85f17611ffe86daf7dc8d7db77a9ffa1710785e137f51fe7ef54c9a311d35ae7d41", + "76e7eda7eec0d7f918f37a2f12a70df9a0267ff20722f80ba7fb8b6231265f16badba17a2d97c9d5cbd45e3e2b25dde32fa07f2f7eafe5f43184cd7f7c0e81e7" + "35f1cdfe632830cd882852e3244bc932cdcf86f5f3df3975fd7725e7bad0fab4407ca4cf7579263fa9839f70ba9fb83e3c08c44e7aa75cb19e6f464e5b07d911", + "9777513eb8ef219e5f568e9788fabd26fe8ace75a90b075173f603d2f91fceb0ed13fa0c02cf6be29bfc052345070c2ba7f99cbaabddb1f91fc8ae3b99c546f4" + "bcd7b7611dd9f9fe213fecc722ad83e15eb9181c37121cef4fd65817f987c788e7d7f59e17545c6535ebc535b5f9d31ab1f5044e6c3243a6496c3da1290cea4e", + "5b2f167ad2962ea7479a4210b4ebbf35fe5d578ffb6d5d0f27750e607456ab25da1146ec07875c62a77c162976298f7bec3af4e3c5ed37eadd8fbe82cbce7e00" + "81e7d538da6b121bb7e3b6eb24ee8150f44157071be2fefff08d375c6dd76dc523355e0f548742a258f515d80425358e65311fcfd051b0eb9bd38f15fa22b678", + "cd4b083caf89bf201f0f3deac504ae47c9cccc12db65f7a716f1ce9078463ebe7dc206b1d971afe39b7ff102f801a7fb814eb327d64eaf138d76287e141a0706" + "54a4b8077e60c3fcc0af62cbcbf661049ed7c437f981d9dbabdf3b75dc9f41e219f978ecff4c5cc4ef630f7ff2a74db0f74eb7f7ed508d2e143bc3663fdd8d95", + "7bd544ae57e5e11cef4d7d2544fd5e13dfaabd57cee77332c3d1d27987667b335b48ba3f2b74886dfcff71049ed7c437e7eb6729e9912a0795efd4f5da3212cf" + "c8c763ffbfa4c86d4b951bf13ccd7f5ffc35f0034ef703317ab7da3bdd6b9cec1ee4af7c27c55eb2705873d17a2df4e7c5a54e46fdfbfc5af8018667199e56f9", + "76edef74a2dea87223ae37bb0f7e1df2f53bdd0f44eb54448ae6828521bb57e46579af7ad9efa7c00f6ca61ff802b63810e4e737e319f9cecdf70af9f9578907" + "f9f9f1d4ef16bbff0d443b97d53f94fdd3f7716edffe72920cab7fa4d472a2959e03edb3567ad276fb096dbe203b769e50bc034fe7e39d27c876ac13ffec6baf", + "81bf70babfe8b5d3526b67541cfaf713338d0cc5fac91337edef778bbfb840b4735579832688fa567dcfaf62ffe58e484b1d816d7af0ad1bdc95a7aed1a1448f" + "079fde549178463e4ebdb9911bd1fb5ec2af27fe1cce7939dd0f649b95fd7c6e6f904a1e36737d215f38895f0760fd7843fd40d1eef3be733f3090e873916e29", + "7f3b362f04d9fda3aadedc929b1dfbce60fd608578a4fc8118c9064b91562cdf48485297aaa43bfb71ce45f9e61e239e5fd773bf1788f6e0cdff4f633be7fb1c" + "02cfab712899e2fd1efbf605595d0f20b17f986695aca17349d961c7ffe4bfed427c675dedf8b2e77c2372eaec6c3c6af85b15d99fdead5c678e2b7b1ef7d871", + "e8c78bdb8f3a0f06ebbff7c32b983e7b4cbfd3f9b0febb2c9e4a9b8207ebbf78ea077bbfb8fdab3afffb09049ed7c45f68ef1fe9a37ce7eefb3cb9034fe7638a" + "dfa872bba53b04edffb7e01e98f5b5ffcb8ef79978824b45a860a42207e31d7fa67fe013861eb0ff9b65ff9fecf784fb01e07e0085e07e0067e0c1fd0078ea87", + "febdf8bd96d3c7b8ebe60f3a3ee9f3c3307f58159e4a9b8207f3073cf53f463cbfaeebbea871f24af23d4f5b6af3df69615b07becbde723de18a16edb3ef56ef" + "813f40e219f996f3c4cee544743de0d7fea51fc6f9eb6acf971de7f7f9fd5ce95810c65785bd14e5abf171a14ebb685f27f45f2319f56ddbee7d9c706f0bdcdb", + "72079e4a9b8207f7b6e0a91fecbe918cfae6df58bb6fceff09767f71a913d87d327860f7f1d40f76df48467dfb0ab6b8cd5df77431d2ec654566e4d4fd9c4748" + "3c23dfda3e80d91f5bbaa4489fe30b7ff78b2988df38dd9ee71a97d5b4986cb492f269a0e12bee44cfb25117e571037b6ea44db1e7b8f3f4833dc78da7d2a6e0", + "813dc7533fd87323a1f6d5a0f0709dafd596551dbbae9a42e219f956f540530392fb647ee3afe05cade3ed78b553a1774bc5d2ce71b938388d676a15a174e2a2" + "fc08d07f8d64d4b7cf6e6c7c1dd65517bfc712761fe2eb2bc483f83a9efac1ee1bc9a86f2f6facdd8775d5c5ef0176df5e3cb0fb78eab71a7f2d983e7b4cbfd3", + "f998ecfec3162bcfff24aa77932ce4d1b188b7323d813c3a1b81077974f0d4bfb6fd789decbde789bd873c0a90474121c8a3e00c3cc8a380a7fe29e2f965e558" + "43d4ef35f171f90975c477de62054a3ed717413d84f5705a837982453c9827e0c6536953f0609e80a7feb5bd070961ff7941ce51b9bc989e75edf6ccf8938d0f", + "5589edef61249ee23de4f6f7e0deaf49260f9fba5f7326293bc6f76f43dc7f7dedfbb2f970fa91419e0b8e4f83dbc7a27f10f3d1fb432ee701fbbee9f67d82a8" + "8f4c5eb4d9f4465027364e5dffb5272fda5c6e9017cd4578a4c6f9f5ce6ee19467e56649e01203fe20504f440f5db46f7f6de7ebebb41e30c9618bebfc2202cf", + "6be29bec3f2d8a82e19eda0b8bed207d2fee4af6fd48b346d2cd2dd5ea6bb6dfec02e692231adff9b3cc19c4779c6ef7477cf1ea3adbe3a2a97e3c29edc50efb" + "4361db45f11dab76ff1051bfd7c4b77c5e4b6c3243a6493fea19db7f81681f5ebdfbd157dc7a0e17f22a18db7bb73ec039dc55e2c1395c3cf5835d5fdc7e945d", + "27a577a6f13c3b90047648938be3e0b6f779249e918f278ea3898be8b98fe90f7f8f077bbfaef67ee9bcf674e98c2f7283684660c7157fb5edaf5ca53ceeb1f7" + "53c4f3b6efd3797a3f7ea8fd951be58cef7381682f563d0c9f618be77f1281e735f1cdf69fea518dcb47a3362d8b2d85efd473bc15249e918f299eafca6d4b95", + "9b32032018d7017fb0423c52fe801352e513b95c2bd4e561e8faf0f228554e9f78dce30fa03f2f2e7532eadf2bd8fcc04b083caf89bfd80f0c24fa5cfdd3aa1f" + "3013aa3d3a39731ca1ebcd13b9e91b7d08ce0fdef71fff19acefaeab3f58765e1e8f1d042397db7efaea2c98291ef74f8ebae59e8bcef7823f585cea64d4bfd7", + "ecf60722ddd2a706d77319d8352f985ac423ec0f6ee4b6a5c98dbc3f80f9c12af148f983cbcaa82d47727290ea9e250e2b9146b31769b9687d608a781efab591" + "8c7ab88b6d7fff0710785e8d23d214cb31bc6dfb7cacae0b64907846bef575604d529a42101cf7bff0e2f360e79d6ee71b72aebceb8fa5fbb9c4617d1818f44f", + "8f7c7e17dd8f38453c0f76de48abb2f3ef43e079350edd93e6a55d76de09f9fe543b3f9314d93ccd7ff7db9f07fbee74fbde64e5c36452eaeda76426ea6f44e2" + "c9ba5475d1fecd29e279b0ef4632ea6188d839dd86c00a769ed375827d57f681cde544d4be877fe76f3f07f6dde9f63d76dd66d35420735a3eec5d777ddb0c43", + "1dd65c947fc7a9fdb78768d7b2faf65e44bb9f37fd5ea540785e4c62d8e2f6909f198567e4437ee667c5536953f0203f339efa9dea072e10edc2ab6ffb309e5f" + "12cfbd7a00e3f955e2c1781e4ffdd07f8d64ccaf105fdbf1bb4d793c19be498fd2bc0ce37a53bb615c6f2f1e8cebf1d4ef547f4034be33390ecfcbe931b171be", + "9e61cdae71fe5b16f1c8e4d76c0df85b39d51422688fbe7bf232d877a7dbf7bd7e285feeb3f19dcb68a75c4c95e47e3cde70d178dfea78ed1851bfd7c4c764df" + "1f363a74e352a4f8366d7c8f0b443bb1ea5f6188cdbedf656f674354657ba563f75792c8cfa1e5d9994bca8673557ffa51d85fe978fbdea6ce42f94035d70fd5", + "23726c87ca07e960d145fbe8c1be2f2e7532e8df0559fb4e8dc0be2f67dfa9910df6fd6db0efceb7ef627fbbd765a46084db2d84f842fd3490a94851f7d877ab" + "f7db3511f57b4d7c5cf67dc63a67d4dcf8e7f3efce9b8cd4a3e499dd5f2a6f305e7dfc2a8ce797c483f13c6e3c9536050fc6f378ea077bbff8bdd6d1dec3f81e", + "c6f7eb627f49e3c1f81e4ffd60ef17bfd752fa387962ef6dca9fcc48d4c9ec03be7e816b9f8e55ff4036afb22646b27995771efcfaf7c13fac088f547fa42f13" + "8d63b9b473c915c6f1d3c076221a39e9c6c03fd8e51f8cbe81a5f9b6dc39e7059ea7dbcddbef75816837567d7ccc63dbcf79cffcca035e62da3cdd8cb194344f", + "b460d77cc159f7251ac446f8bec4f0bf83fb12d7d72f2c9b5fb990956ba956b798d9cd253b7bfbdd71feb49ef4805f5877bfd043b47b25fb3aa77c785ebef3c4" + "4f5c20ea87b8914a1037c28da7d2a6e041dc084ffd6eb5ff178876e3d54701e24610375ad87e881bd98b0771233cf5837f58fc5e4be9e3f77ad8fcc38b083caf", + "89bfc82e17d88164d7f92fabf1a22212cfc8c7e4077471d9710eacf26119fc81d3fdc141e9e0f4b414c857e51495bfeca633d9f6ce958bf6154d11cf2f2bc70b" + "44fd5e131fe73ab31a03362e37935d3fa8635b3ff83402cf6be26bebebe72d416405a1772e0c69b1c50a57e7f38313d6e3466642b5472727e98fe61e9e223f92", + "f91fa6dff9c173104772ba5f90328951227e7d5d381cf98359ff76ac18f16d437efe9bfa36337ff39eddebcadae55c0cdf12ce65856f975f70e6bd6eaadc08df" + "dbfbfe3f7f05d695d7d51f2cbbae9c0f1dd7528da21ce003a35833d9a1d3ec20ea017fb0aefe00a517cbea21eafe147d3df945c3b7af86d532a29551882359c4", + "8338d2caf0e6b429781047c253ff14f1bc53fd430fd1ded5dc13f05a582dd31057f2405c69d1fb405cc95e3c882be1a97f8a78dea97e82ec3ce22b61b58c6a25" + "3e7ff131443bbc26fea2794496e10792c677ea7c6265f9aed0f389b9d86c8853c27c628578a4fc4497db8926a2e3628de686ac1ca473c9c0fec045fb58a788e7", + "9dea27c8ce27c261b5ccaead7f58977dade03716973a81df2083077e034ffd53c4f34ef51b1788f6e2cd97115e0b3f5166381afc04363f311727797d0a839f58" + "211e293fc19d358e02bb8552eaa877186ac6b3fda150edb8e8fe8329e279a7fa89c788f62eab870f4c9f75d2e717e1db5f4ee2eac78936cf984435765a2b13da", + "f759add43e7b726bb7eeed16ff02ebe12bc39bd3a6e0c17a389efaa788e79dea5f2e10edc5ab87516cf3908f23f0bc26bec92ed7594a7ac48ca8792e0e8548ad" + "67480cdfc6b99e5146e219f998f44791db962a37557308ce377ef6b5d760dddbe9fea05e154e8e0aa7ad3a77745daa45ebb1b674d276d17c03faf3e25227a3fe", + "f9d7c30ff02cc32f791fd1ba9ea3b0456fe67223ae37bb907fc3f97e205aa7225234172c0cd9bd222fcb7bd5cb7edf45f30247f667dbfc40109b1ff80c02cf6b" + "e22ff20303893e9f4d9094bf1dbb1ff6ec0e3c9d8f536f6ec9cd8e3811f88315e211cbd717c9064b91562cdf48485297aaa43bfb71ce45f998c01f2c2e7532ea", + "5f089b3ff81402cf6be29bfc816ece6e42444e8d0f559178463ebef8a2517508c687be0579bb9def07f8fa553542b3f5d0a01c4a0a27c99da3f1d10ecc0bd6ae" + "3ff710ed5ccd3ed750582d5f5917bf306a5075c9e701bff08c7aa4ca0dfc826bf048f985f2c941f972e4a3bb3b8118dfd9dee712c79d36cc0f36b03f2bf4288c", + "cbeea2cee57935ceecf5e6a55d76fef72de2a5907846beb5fb1a5a037e6b2629b279b8a7272f835d77ba5defd7ebcd56b057cf970b25b128f85229f628e8a273" + "0a0eb5eb36ccdf15c297570ff2623c1d4fe7435e8cfbe2a9b429789017034ffde00f16973a19f56f1fce11c03982a7bec7127a04e708568807e708f0d43f453c", + "0fe7088c64d4c3a4dde799d5c500e98ad2d7439cba1e5042e219f938f717cce5a6290ec179029c2358211ea93cdcc3087b1c901ac55d4918b395417d28957c55" + "8f7bfc01f4e7c5a54e46fdc3b72e0ce7089e8e07e708ee8d37a74dc183730478ea073fb0b8d4c9a87fafc139020f9ef9249c235839de9c36050fce11e0a91ffc", + "c1e25227a3fe7d059b3fb8e73d6d37fb45f5109153e34384ef69bbd95fa6ab0ec1f8d0e7fef525ec2b72ba1fa8f7a29da3743e1f392cb452c7fee08e78249cc2" + "bc60edfab31e37bfaffe3ddb39027f582d03b6ed1f5d97f563d8576a24d8574a060ff695e2a91ffaaf918cfaf692dd7120468a0e18564ef3b901478b4c63edec", + "bfd5f1c3e91dedd0f998f60f99c449725fe9e4db3ffe6bf0074ef707f9613f16691d0cf7cac5e0b891e0787fb2c61eb8c71f4c11cf2f2bc7d711f57b4d7c3cfd" + "99eaf5d871a941b194981cf00d9911785be2430776ef1f92e63278a4cf12d66f9ee0b078a32a4f6dcca110c179c337ffe615f0134ef713dd9d54b1dd2c56d2fb", + "c99c6fbf522feeb45b05583fb8a96f5de246178876ae6b5e3a388ff6743c9d0fe7d1ee8ba7d2a6e0c179343cf5833f585cea64d4bf5dbbe34aea3e53293760d9" + "04d793c7c4d693cdf7304f2de2d9b1bfe896dcecd85ff467df8475e5b5f5074b9f3bc852d7f95eb048cbd9d13117124fa94aacec718f3f98229e877e6d24a31e", + "be8c6d3df903083cafc66124ba3fa058dbecfe5b16f132483c23dfdabad3ec8f2d4d52c4e34093b761bd607dedfcb2e3fed2497f20e7d295cac971a9b82bf623" + "073b34e5a2f563e8c78bdb8f5a47ee21ea5b565ea87f1fafa99cc72fd4373e6f0822edd4f1fd57917846bea21f056b0ad213992125d35b66d1cd35869cbe8443", + "7fce42bcc7e9767fbf19a1e22166ffaa58ae34babe83da553dd57751bc678a781efab5918c7af805bbf310f194d866f846e7d2a9f1ff37eec0d3f9cf345e903a" + "944837b7d459a13637344f116f2447fa7cf1e485578ee13c99d3fdc1787495dea792adf6ce76239f104adde42074e5227ff03dc4f3cbca9145d4ef35f157ea0f", + "1ed69959371febdfde7ebf0b44fbb1eae5f4045bfce781e9b3e7e6772a8791a41e254ab6cd0bacce1b73483c23dfcabc519190b2f3589715d975a2f08357df84" + "7980d3edfe55352d25d9cb4ad2df124becb09b3fe4af8bb00fe8a6be7559f745c56556737e2c1c9e1793a8ddf301c84b0a79499f156f4e9b82077949f1d43f45", + "3c0f79498d64d0c3491a9b7ff83002cf6be29bec729319aadf939a2798f34d589d27ac6c7d69a1deccc4457c7d29fcc99f36619ee074fbdf0ed5e842b1336cf6" + "d3dd58b9574de47a55de45f1a129e279b0ff4632ea61d6ee73656ad2a1362d0e34be5d7ec061e7c7d449a62237f2fb867ef87b3cf803a7fb83ce3ecfe57dcdf1", + "2e5767a4ee91183df34971881b6d607f56288ecd0f405eeaa7e3415eea7be3cd6953f0202f359efadde207c8ae1f24c2f362928073641e3cf3cb0d3c6f02e7c8" + "568807e7c8f0d4ef16ff70816827def58227f798d9b49e7c132f726a9ce8188967e4e3d617f2f1458813ad128fd4fc60781aeaec965ac56ccb47b5b2b5613cd4", + "dd0fb968dd00faf3e25227a3fe45214ee48138d1ed76439cc85e3c8813e1a9df2d7e806c9c28169e179318c4893c10275af43e1027b2170fe24478ea778b7fb8" + "40b4136f9c08df7a32dc6ff9743cb8dfd232de9c36050feeb7c4533ff883c5a54ea8758309a2be65e5f609049ed7c447dd6f3913c19cefd4f58313249e918ffb", + "3ca3ae3a04cf1d7cabfa31981738dd0f04e3996ce0b812f4b73ba5f4b594df2e8da5b28bf2d3417f5e5cea64d4bf47d8fc80d57b8e67ef3fef024ef503769d6b" + "57e406f71cbb078f941f18a50a7bfed4e8f8f268bf56f497f367a3d24e1efcc0daf567a2eb07936858fde3c9fac105a27eb8e7f8e9ed70ef3da970cff12af1e0", + "9e633cf5bbc5fe5f20da89371e846fbdf89ee78ee7f637cbf003f37d961716dbf3acebc5ce8a233e119b0d7144c843b4423c62f756723bd144745cacd1dc9095" + "83742e19d81f801f583b3f40761e100fab7f64b0f905b8c7f2e9783a1feeb1bc2f9e4a9b8207f758e2a91ffc834af73b8f8ccf3fc03e53149e910ffb4cef8d37", + "a74dc1837da678ea778b7fb840b413affe3df107eb92dffac2623b48c78f208ff5caf0e6b4297890c71a4ffd6eb1ff13443b97d5bff79b3eeba4cf0f5ebafde5" + "a41856ffc8a8e524af7d3e5e3bffb02eebcde03716973a81df2083077e034ffd53c4f3abb82fd37a7f9ef564811d2a37a25d51227fae6c8e1cb094e821ad8725", + "6cfe61593d34d965e5f5672f8f6ffd19e537bd1aa7d1998b199bdeac246ffa32f7656a92b3611dfa3fc0b904e7fb831d71c4060252a479581f50f956fcaa50b9" + "665db40e3d453cbf9ef7213cf107da5fb951cef83e1788f662d5c3700d9b3fb8e7385d6a50333f9868db762ee12d8b7805d3678fe9773a1f8fdee8e222b9ce3c", + "f9a397e1de4cc7dbffede3b66f281fee0b62bb239592543b9a4c0fe360ffd7d5fef710ed5dc97a73f88df0bc9cbe61f73e55f5b09a2c4acbc63dd6f5bc9a2d79" + "7215b911cf933bfdd9d75e03ffe074ff10f15f869841a31118d4a357570d9fcf1ff5a5217fc506f667850eb1f901c87bfa743cc87b7a6fbc396d0a1ee43dc553", + "3ff881c5a54e46fd3b5a8bfda790d70ef2daadbb7d268d0779edf0d40ffe6071a99351fff2b05e60110fd60b70e3a9b42978b05e80a77eb0fb8b4b9d8cf92bb2" + "d8e60156f3d8e94b034e5d17b06bbfb2ae3a04fdc04ffeb100fb869cee074e73f1c476960a1c9c94837b5ca6d8c85e1e6d433c6803fbb3424118ff5bc483f13f", + "6e3c9536050fc6ff78ea778bddff3788762eab7f1f40bc87be4f68eff697ef34c36ad9d04a4a2b2fd4324c6b655d2d5f6a407e23ed37d37be2e9f55f983e9bf1" + "743ee437ba2f9e4a9b8207f98df0d4ef163f728168275efd6b609b3ffc2202cf6be2bf2b8ed4607acaf74e9d3f649178463e1e7d998b8bf4bd19ffe94710375a", + "5bbbbf6cdea25e22173faa8cb7f7affba54ca33f4aec1ce7da1ef7d8fd29e279386f60a4c5e70dbe1a56ff7813e6071e981f2c7a1f981fd88b07f3036bf5ff7f" + "2360be70", "" }; + + nameCaptureInfo = NULL; + emlrtNameCaptureMxArrayR2016a(data, 331168U, &nameCaptureInfo); + return nameCaptureInfo; +} + +/* + * Arguments : void + * Return Type : mxArray * + */ +mxArray *emlrtMexFcnProperties() +{ + mxArray *xResult; + mxArray *xEntryPoints; + const char * fldNames[6] = { "Name", "NumberOfInputs", "NumberOfOutputs", + "ConstantInputs", "FullPath", "TimeStamp" }; + + mxArray *xInputs; + const char * b_fldNames[4] = { "Version", "ResolvedFunctions", "EntryPoints", + "CoverageInfo" }; + + xEntryPoints = emlrtCreateStructMatrix(1, 1, 6, *(const char * (*)[6])& + fldNames[0]); + xInputs = emlrtCreateLogicalMatrix(1, 22); + emlrtSetField(xEntryPoints, 0, "Name", emlrtMxCreateString("QEKF")); + emlrtSetField(xEntryPoints, 0, "NumberOfInputs", emlrtMxCreateDoubleScalar + (22.0)); + emlrtSetField(xEntryPoints, 0, "NumberOfOutputs", emlrtMxCreateDoubleScalar + (2.0)); + emlrtSetField(xEntryPoints, 0, "ConstantInputs", xInputs); + emlrtSetField(xEntryPoints, 0, "FullPath", emlrtMxCreateString( + "C:\\Users\\Thomas\\Documents\\Kugle-MATLAB\\Estimators\\QEKF\\QEKF.m")); + emlrtSetField(xEntryPoints, 0, "TimeStamp", emlrtMxCreateDoubleScalar + (737531.74909722223)); + xResult = emlrtCreateStructMatrix(1, 1, 4, *(const char * (*)[4])&b_fldNames[0]); + emlrtSetField(xResult, 0, "Version", emlrtMxCreateString( + "9.4.0.813654 (R2018a)")); + emlrtSetField(xResult, 0, "ResolvedFunctions", (mxArray *) + emlrtMexFcnResolvedFunctionsInfo()); + emlrtSetField(xResult, 0, "EntryPoints", xEntryPoints); + return xResult; +} + +/* + * File trailer for _coder_QEKF_info.c + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_info.h b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_info.h new file mode 100644 index 0000000..08c9594 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_info.h @@ -0,0 +1,28 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_QEKF_info.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +#ifndef _CODER_QEKF_INFO_H +#define _CODER_QEKF_INFO_H +/* Include Files */ +#include "tmwtypes.h" +#include "mex.h" +#include "emlrt.h" + + +/* Function Declarations */ +extern const mxArray *emlrtMexFcnResolvedFunctionsInfo(); +MEXFUNCTION_LINKAGE mxArray *emlrtMexFcnProperties(); + +#endif +/* + * File trailer for _coder_QEKF_info.h + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_mex.cpp b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_mex.cpp new file mode 100644 index 0000000..6f7bd5b --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_mex.cpp @@ -0,0 +1,101 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_QEKF_mex.cpp + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +/* Include Files */ +#include "_coder_QEKF_api.h" +#include "_coder_QEKF_mex.h" + +/* Function Declarations */ +static void QEKF_mexFunction(int32_T nlhs, mxArray *plhs[2], int32_T nrhs, const + mxArray *prhs[22]); + +/* Function Definitions */ + +/* + * Arguments : int32_T nlhs + * mxArray *plhs[2] + * int32_T nrhs + * const mxArray *prhs[22] + * Return Type : void + */ +static void QEKF_mexFunction(int32_T nlhs, mxArray *plhs[2], int32_T nrhs, const + mxArray *prhs[22]) +{ + const mxArray *outputs[2]; + int32_T b_nlhs; + emlrtStack st = { NULL, /* site */ + NULL, /* tls */ + NULL /* prev */ + }; + + st.tls = emlrtRootTLSGlobal; + + /* Check for proper number of arguments. */ + if (nrhs != 22) { + emlrtErrMsgIdAndTxt(&st, "EMLRT:runTime:WrongNumberOfInputs", 5, 12, 22, 4, + 4, "QEKF"); + } + + if (nlhs > 2) { + emlrtErrMsgIdAndTxt(&st, "EMLRT:runTime:TooManyOutputArguments", 3, 4, 4, + "QEKF"); + } + + /* Call the function. */ + QEKF_api(prhs, nlhs, outputs); + + /* Copy over outputs to the caller. */ + if (nlhs < 1) { + b_nlhs = 1; + } else { + b_nlhs = nlhs; + } + + emlrtReturnArrays(b_nlhs, plhs, outputs); + + /* Module termination. */ + QEKF_terminate(); +} + +/* + * Arguments : int32_T nlhs + * mxArray * const plhs[] + * int32_T nrhs + * const mxArray * const prhs[] + * Return Type : void + */ +void mexFunction(int32_T nlhs, mxArray *plhs[], int32_T nrhs, const mxArray + *prhs[]) +{ + mexAtExit(QEKF_atexit); + + /* Initialize the memory manager. */ + /* Module initialization. */ + QEKF_initialize(); + + /* Dispatch the entry-point. */ + QEKF_mexFunction(nlhs, plhs, nrhs, prhs); +} + +/* + * Arguments : void + * Return Type : emlrtCTX + */ +emlrtCTX mexFunctionCreateRootTLS(void) +{ + emlrtCreateRootTLS(&emlrtRootTLSGlobal, &emlrtContextGlobal, NULL, 1); + return emlrtRootTLSGlobal; +} + +/* + * File trailer for _coder_QEKF_mex.cpp + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_mex.h b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_mex.h new file mode 100644 index 0000000..04dd63a --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/interface/_coder_QEKF_mex.h @@ -0,0 +1,34 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_QEKF_mex.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +#ifndef _CODER_QEKF_MEX_H +#define _CODER_QEKF_MEX_H + +/* Include Files */ +#include +#include +#include +#include "tmwtypes.h" +#include "mex.h" +#include "emlrt.h" +#include "_coder_QEKF_api.h" + +/* Function Declarations */ +extern void mexFunction(int32_T nlhs, mxArray *plhs[], int32_T nrhs, const + mxArray *prhs[]); +extern emlrtCTX mexFunctionCreateRootTLS(void); + +#endif + +/* + * File trailer for _coder_QEKF_mex.h + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/mrdivide.cpp b/Estimators/QEKF/codegen/lib/QEKF/mrdivide.cpp new file mode 100644 index 0000000..cb0f61a --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/mrdivide.cpp @@ -0,0 +1,291 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: mrdivide.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// + +// Include Files +#include +#include "rt_nonfinite.h" +#include "QEKF.h" +#include "mrdivide.h" + +// Function Definitions + +// +// Arguments : const float A[60] +// const float B[36] +// float y[60] +// Return Type : void +// +void b_mrdivide(const float A[60], const float B[36], float y[60]) +{ + int k; + int j; + int jAcol; + int c; + float b_A[36]; + int jy; + int ix; + signed char ipiv[6]; + static float b_B[60]; + float smax; + int iy; + int i; + float s; + for (k = 0; k < 6; k++) { + for (jAcol = 0; jAcol < 6; jAcol++) { + b_A[jAcol + 6 * k] = B[k + 6 * jAcol]; + } + + for (jAcol = 0; jAcol < 10; jAcol++) { + b_B[jAcol + 10 * k] = A[k + 6 * jAcol]; + } + + ipiv[k] = (signed char)(1 + k); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + jAcol = 0; + ix = c; + smax = (float)fabs((double)b_A[c]); + for (k = 2; k <= 6 - j; k++) { + ix++; + s = (float)fabs((double)b_A[ix]); + if (s > smax) { + jAcol = k - 1; + smax = s; + } + } + + if (b_A[c + jAcol] != 0.0F) { + if (jAcol != 0) { + ipiv[j] = (signed char)((j + jAcol) + 1); + ix = j; + iy = j + jAcol; + for (k = 0; k < 6; k++) { + smax = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = smax; + ix += 6; + iy += 6; + } + } + + k = (c - j) + 6; + for (i = c + 1; i < k; i++) { + b_A[i] /= b_A[c]; + } + } + + iy = c; + jy = c + 6; + for (jAcol = 1; jAcol <= 5 - j; jAcol++) { + smax = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + k = (iy - j) + 12; + for (i = 7 + iy; i < k; i++) { + b_A[i] += b_A[ix] * -smax; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + for (j = 0; j < 6; j++) { + jy = 10 * j; + jAcol = 6 * j; + for (k = 1; k <= j; k++) { + iy = 10 * (k - 1); + if (b_A[(k + jAcol) - 1] != 0.0F) { + for (i = 0; i < 10; i++) { + b_B[i + jy] -= b_A[(k + jAcol) - 1] * b_B[i + iy]; + } + } + } + + smax = 1.0F / b_A[j + jAcol]; + for (i = 0; i < 10; i++) { + b_B[i + jy] *= smax; + } + } + + for (j = 5; j >= 0; j--) { + jy = 10 * j; + jAcol = 6 * j - 1; + for (k = j + 2; k < 7; k++) { + iy = 10 * (k - 1); + if (b_A[k + jAcol] != 0.0F) { + for (i = 0; i < 10; i++) { + b_B[i + jy] -= b_A[k + jAcol] * b_B[i + iy]; + } + } + } + } + + for (jAcol = 4; jAcol >= 0; jAcol--) { + if (ipiv[jAcol] != jAcol + 1) { + iy = ipiv[jAcol] - 1; + for (jy = 0; jy < 10; jy++) { + smax = b_B[jy + 10 * jAcol]; + b_B[jy + 10 * jAcol] = b_B[jy + 10 * iy]; + b_B[jy + 10 * iy] = smax; + } + } + } + + for (k = 0; k < 10; k++) { + for (jAcol = 0; jAcol < 6; jAcol++) { + y[jAcol + 6 * k] = b_B[k + 10 * jAcol]; + } + } +} + +// +// Arguments : const float A[70] +// const float B[49] +// float y[70] +// Return Type : void +// +void mrdivide(const float A[70], const float B[49], float y[70]) +{ + int k; + int j; + int jAcol; + int c; + static float b_A[49]; + int jy; + int ix; + signed char ipiv[7]; + static float b_B[70]; + float smax; + int iy; + int i; + float s; + for (k = 0; k < 7; k++) { + for (jAcol = 0; jAcol < 7; jAcol++) { + b_A[jAcol + 7 * k] = B[k + 7 * jAcol]; + } + + for (jAcol = 0; jAcol < 10; jAcol++) { + b_B[jAcol + 10 * k] = A[k + 7 * jAcol]; + } + + ipiv[k] = (signed char)(1 + k); + } + + for (j = 0; j < 6; j++) { + c = j << 3; + jAcol = 0; + ix = c; + smax = (float)fabs((double)b_A[c]); + for (k = 2; k <= 7 - j; k++) { + ix++; + s = (float)fabs((double)b_A[ix]); + if (s > smax) { + jAcol = k - 1; + smax = s; + } + } + + if (b_A[c + jAcol] != 0.0F) { + if (jAcol != 0) { + ipiv[j] = (signed char)((j + jAcol) + 1); + ix = j; + iy = j + jAcol; + for (k = 0; k < 7; k++) { + smax = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = smax; + ix += 7; + iy += 7; + } + } + + k = (c - j) + 7; + for (i = c + 1; i < k; i++) { + b_A[i] /= b_A[c]; + } + } + + iy = c; + jy = c + 7; + for (jAcol = 1; jAcol <= 6 - j; jAcol++) { + smax = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + k = (iy - j) + 14; + for (i = 8 + iy; i < k; i++) { + b_A[i] += b_A[ix] * -smax; + ix++; + } + } + + jy += 7; + iy += 7; + } + } + + for (j = 0; j < 7; j++) { + jy = 10 * j; + jAcol = 7 * j; + for (k = 1; k <= j; k++) { + iy = 10 * (k - 1); + if (b_A[(k + jAcol) - 1] != 0.0F) { + for (i = 0; i < 10; i++) { + b_B[i + jy] -= b_A[(k + jAcol) - 1] * b_B[i + iy]; + } + } + } + + smax = 1.0F / b_A[j + jAcol]; + for (i = 0; i < 10; i++) { + b_B[i + jy] *= smax; + } + } + + for (j = 6; j >= 0; j--) { + jy = 10 * j; + jAcol = 7 * j - 1; + for (k = j + 2; k < 8; k++) { + iy = 10 * (k - 1); + if (b_A[k + jAcol] != 0.0F) { + for (i = 0; i < 10; i++) { + b_B[i + jy] -= b_A[k + jAcol] * b_B[i + iy]; + } + } + } + } + + for (jAcol = 5; jAcol >= 0; jAcol--) { + if (ipiv[jAcol] != jAcol + 1) { + iy = ipiv[jAcol] - 1; + for (jy = 0; jy < 10; jy++) { + smax = b_B[jy + 10 * jAcol]; + b_B[jy + 10 * jAcol] = b_B[jy + 10 * iy]; + b_B[jy + 10 * iy] = smax; + } + } + } + + for (k = 0; k < 10; k++) { + for (jAcol = 0; jAcol < 7; jAcol++) { + y[jAcol + 7 * k] = b_B[k + 10 * jAcol]; + } + } +} + +// +// File trailer for mrdivide.cpp +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/mrdivide.h b/Estimators/QEKF/codegen/lib/QEKF/mrdivide.h new file mode 100644 index 0000000..5823ea3 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/mrdivide.h @@ -0,0 +1,29 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: mrdivide.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// +#ifndef MRDIVIDE_H +#define MRDIVIDE_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "QEKF_types.h" + +// Function Declarations +extern void b_mrdivide(const float A[60], const float B[36], float y[60]); +extern void mrdivide(const float A[70], const float B[49], float y[70]); + +#endif + +// +// File trailer for mrdivide.h +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/mw_cmsis.h b/Estimators/QEKF/codegen/lib/QEKF/mw_cmsis.h new file mode 100644 index 0000000..a95ed5b --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/mw_cmsis.h @@ -0,0 +1,84 @@ +/* Copyright 2015 The MathWorks, Inc. */ + +/**************************************************** +* * +* wrapper fuctions for CMSIS functions * +* * +****************************************************/ + +#ifndef MW_CMSIS_H +#define MW_CMSIS_H + +#include "arm_math.h" +#include "rtwtypes.h" + +#define mw_arm_abs_f32(pSrc, pDst, blockSize) arm_abs_f32((float32_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_abs_q7(pSrc, pDst, blockSize) arm_abs_q7((q7_t *)pSrc, (q7_t *)pDst, blockSize) +#define mw_arm_abs_q15(pSrc, pDst, blockSize) arm_abs_q15((q15_t *)pSrc, (q15_t *)pDst, blockSize) +#define mw_arm_abs_q31(pSrc, pDst, blockSize) arm_abs_q31((q31_t *)pSrc, (q31_t *)pDst, blockSize) + +#define mw_arm_sqrt_q15(in, pOut) arm_sqrt_q15((q15_t)in,(q15_t *)pOut) +#define mw_arm_sqrt_q31(in, pOut) arm_sqrt_q31((q31_t)in,(q31_t *)pOut) +#define mw_arm_sqrt_f32(in, pOut) arm_sqrt_f32((float32_t)in,(float32_t *)pOut) + +#define mw_arm_float_to_q31(pSrc, pDst, blockSize) arm_float_to_q31((float32_t *)pSrc, (q31_t *)pDst, blockSize) +#define mw_arm_float_to_q15(pSrc, pDst, blockSize) arm_float_to_q15((float32_t *)pSrc, (q15_t *)pDst, blockSize) +#define mw_arm_float_to_q7(pSrc, pDst, blockSize) arm_float_to_q7((float32_t *)pSrc, (q7_t *)pDst, blockSize) + +#define mw_arm_q15_to_float(pSrc, pDst, blockSize) arm_q15_to_float((q15_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_q15_to_q31(pSrc, pDst, blockSize) arm_q15_to_q31((q15_t *)pSrc, (q31_t *)pDst, blockSize) +#define mw_arm_q15_to_q7(pSrc, pDst, blockSize) arm_q15_to_q7((q15_t *)pSrc, (q7_t *)pDst, blockSize) + +#define mw_arm_q31_to_float(pSrc, pDst, blockSize) arm_q31_to_float((q31_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_q31_to_q15(pSrc, pDst, blockSize) arm_q31_to_q15((q31_t *)pSrc, (q15_t *)pDst, blockSize) +#define mw_arm_q31_to_q7(pSrc, pDst, blockSize) arm_q31_to_q7((q31_t *)pSrc, (q7_t *)pDst, blockSize) + +#define mw_arm_q7_to_float(pSrc, pDst, blockSize) arm_q7_to_float((q7_t *)pSrc, (float32_t *)pDst, blockSize) +#define mw_arm_q7_to_q31(pSrc, pDst, blockSize) arm_q7_to_q31((q7_t *)pSrc, (q31_t *)pDst, blockSize) +#define mw_arm_q7_to_q15(pSrc, pDst, blockSize) arm_q7_to_q15((q7_t *)pSrc, (q15_t *)pDst, blockSize) + +#define mw_arm_add_f32(pSrcA, pSrcB, pDst, blockSize) arm_add_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_add_q31(pSrcA, pSrcB, pDst, blockSize) arm_add_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_add_q15(pSrcA, pSrcB, pDst, blockSize) arm_add_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) +#define mw_arm_add_q7(pSrcA, pSrcB, pDst, blockSize) arm_add_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) + +#define mw_arm_sub_f32(pSrcA, pSrcB, pDst, blockSize) arm_sub_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_sub_q31(pSrcA, pSrcB, pDst, blockSize) arm_sub_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_sub_q15(pSrcA, pSrcB, pDst, blockSize) arm_sub_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) +#define mw_arm_sub_q7(pSrcA, pSrcB, pDst, blockSize) arm_sub_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) + +#define mw_arm_mult_f32(pSrcA, pSrcB, pDst, blockSize) arm_mult_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_mult_q31(pSrcA, pSrcB, pDst, blockSize) arm_mult_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_mult_q15(pSrcA, pSrcB, pDst, blockSize) arm_mult_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) +#define mw_arm_mult_q7(pSrcA, pSrcB, pDst, blockSize) arm_mult_q7((q7_t *)pSrcA, (q7_t *)pSrcB, (q7_t *)pDst, blockSize) + +#define mw_arm_cmplx_conj_f32(pSrc, pDst, numSamples) arm_cmplx_conj_f32((float32_t *)pSrc, (float32_t *)pDst, numSamples) +#define mw_arm_cmplx_conj_q31(pSrc, pDst, numSamples) arm_cmplx_conj_q31((q31_t *)pSrc, (q31_t *)pDst, numSamples) +#define mw_arm_cmplx_conj_q15(pSrc, pDst, numSamples) arm_cmplx_conj_q15((q15_t *)pSrc, (q15_t *)pDst, numSamples) + +#define mw_arm_cmplx_mult_cmplx_f32(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_cmplx_q31(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_cmplx_q15(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_cmplx_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) + +#define mw_arm_cmplx_mult_real_f32(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_f32((float32_t *)pSrcA, (float32_t *)pSrcB, (float32_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_real_q31(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_q31((q31_t *)pSrcA, (q31_t *)pSrcB, (q31_t *)pDst, blockSize) +#define mw_arm_cmplx_mult_real_q15(pSrcA, pSrcB, pDst, blockSize) arm_cmplx_mult_real_q15((q15_t *)pSrcA, (q15_t *)pSrcB, (q15_t *)pDst, blockSize) + +#define mw_arm_rshift_q15(pSrc, shiftBits, pDst, blockSize) arm_shift_q15 ((q15_t *)pSrc, -(shiftBits),(q15_t *)pDst, blockSize) +#define mw_arm_rshift_q31(pSrc, shiftBits, pDst, blockSize) arm_shift_q31 ((q31_t *)pSrc, -(shiftBits), (q31_t *)pDst, blockSize) +#define mw_arm_rshift_q7(pSrc, shiftBits, pDst, blockSize) arm_shift_q7 ((q7_t *)pSrc, -(shiftBits), (q7_t *)pDst, blockSize) + +#define mw_arm_shift_q15(pSrc, shiftBits, pDst, blockSize) arm_shift_q15 ((q15_t *)pSrc, shiftBits,(q15_t *)pDst, blockSize) +#define mw_arm_shift_q31(pSrc, shiftBits, pDst, blockSize) arm_shift_q31 ((q31_t *)pSrc, shiftBits, (q31_t *)pDst, blockSize) +#define mw_arm_shift_q7(pSrc, shiftBits, pDst, blockSize) arm_shift_q7 ((q7_t *)pSrc, shiftBits, (q7_t *)pDst, blockSize) + +/* Wrapper function prototypes for Matrix Addition */ +void mw_arm_mat_add_f32(real32_T * pSrcA, real32_T * pSrcB, real32_T * pDst, uint16_t nRows, uint16_t nCols); +void mw_arm_mat_add_q15(int16_T * pSrcA, int16_T * pSrcB, int16_T * pDst, uint16_t nRows, uint16_t nCols); +void mw_arm_mat_add_q31(int32_T * pSrcA, int32_T * pSrcB, int32_T * pDst, uint16_t nRows, uint16_t nCols); +/* Wrapper function prototypes for Matrix Subtraction */ +void mw_arm_mat_sub_f32(real32_T * pSrcA, real32_T * pSrcB, real32_T * pDst, uint16_t nRows, uint16_t nCols); +void mw_arm_mat_sub_q15(int16_T * pSrcA, int16_T * pSrcB, int16_T * pDst, uint16_t nRows, uint16_t nCols); +void mw_arm_mat_sub_q31(int32_T * pSrcA, int32_T * pSrcB, int32_T * pDst, uint16_t nRows, uint16_t nCols); + +#endif diff --git a/Estimators/QEKF/codegen/lib/QEKF/norm.cpp b/Estimators/QEKF/codegen/lib/QEKF/norm.cpp new file mode 100644 index 0000000..e2b2d14 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/norm.cpp @@ -0,0 +1,84 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: norm.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// + +// Include Files +#include +#include "mw_cmsis.h" +#include "rt_nonfinite.h" +#include "QEKF.h" +#include "norm.h" + +// Function Definitions + +// +// Arguments : const float x[4] +// Return Type : float +// +float b_norm(const float x[4]) +{ + float y; + float scale; + int k; + float f1; + float absxk; + float t; + y = 0.0F; + scale = 1.29246971E-26F; + for (k = 0; k < 4; k++) { + absxk = (float)fabs((double)x[k]); + if (absxk > scale) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + + mw_arm_sqrt_f32(y, &f1); + return scale * f1; +} + +// +// Arguments : const float x[3] +// Return Type : float +// +float norm(const float x[3]) +{ + float y; + float scale; + int k; + float f0; + float absxk; + float t; + y = 0.0F; + scale = 1.29246971E-26F; + for (k = 0; k < 3; k++) { + absxk = (float)fabs((double)x[k]); + if (absxk > scale) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + + mw_arm_sqrt_f32(y, &f0); + return scale * f0; +} + +// +// File trailer for norm.cpp +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/norm.h b/Estimators/QEKF/codegen/lib/QEKF/norm.h new file mode 100644 index 0000000..b1fc8c6 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/norm.h @@ -0,0 +1,29 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: norm.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// +#ifndef NORM_H +#define NORM_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "QEKF_types.h" + +// Function Declarations +extern float b_norm(const float x[4]); +extern float norm(const float x[3]); + +#endif + +// +// File trailer for norm.h +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/rtGetInf.cpp b/Estimators/QEKF/codegen/lib/QEKF/rtGetInf.cpp new file mode 100644 index 0000000..0ad9f3f --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/rtGetInf.cpp @@ -0,0 +1,129 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rtGetInf.cpp + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, Inf and MinusInf + */ +#include "rtGetInf.h" + +/* Function: rtGetInf ================================================== + * Abstract: + * Initialize rtInf needed by the generated code. + */ +real_T rtGetInf(void) +{ + real_T inf = 0.0; + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + } + + return inf; +} + +/* Function: rtGetInfF ================================================== + * Abstract: + * Initialize rtInfF needed by the generated code. + */ +real32_T rtGetInfF(void) +{ + IEEESingle infF; + infF.wordL.wordLuint = 0x7F800000U; + return infF.wordL.wordLreal; +} + +/* Function: rtGetMinusInf ================================================== + * Abstract: + * Initialize rtMinusInf needed by the generated code. + */ +real_T rtGetMinusInf(void) +{ + real_T minf = 0.0; + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + } + + return minf; +} + +/* Function: rtGetMinusInfF ================================================== + * Abstract: + * Initialize rtMinusInfF needed by the generated code. + */ +real32_T rtGetMinusInfF(void) +{ + IEEESingle minfF; + minfF.wordL.wordLuint = 0xFF800000U; + return minfF.wordL.wordLreal; +} + +/* + * File trailer for rtGetInf.cpp + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/rtGetInf.h b/Estimators/QEKF/codegen/lib/QEKF/rtGetInf.h new file mode 100644 index 0000000..03a35e1 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/rtGetInf.h @@ -0,0 +1,28 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rtGetInf.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +#ifndef RTGETINF_H +#define RTGETINF_H +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetInf(void); +extern real32_T rtGetInfF(void); +extern real_T rtGetMinusInf(void); +extern real32_T rtGetMinusInfF(void); + +#endif + +/* + * File trailer for rtGetInf.h + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/rtGetNaN.cpp b/Estimators/QEKF/codegen/lib/QEKF/rtGetNaN.cpp new file mode 100644 index 0000000..77ff001 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/rtGetNaN.cpp @@ -0,0 +1,96 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rtGetNaN.cpp + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, NaN + */ +#include "rtGetNaN.h" + +/* Function: rtGetNaN ================================================== + * Abstract: + * Initialize rtNaN needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetNaN(void) +{ + real_T nan = 0.0; + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF80000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + nan = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; + tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; + nan = tmpVal.fltVal; + break; + } + } + + return nan; +} + +/* Function: rtGetNaNF ================================================== + * Abstract: + * Initialize rtNaNF needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetNaNF(void) +{ + IEEESingle nanF = { { 0 } }; + + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + nanF.wordL.wordLuint = 0xFFC00000U; + break; + } + + case BigEndian: + { + nanF.wordL.wordLuint = 0x7FFFFFFFU; + break; + } + } + + return nanF.wordL.wordLreal; +} + +/* + * File trailer for rtGetNaN.cpp + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/rtGetNaN.h b/Estimators/QEKF/codegen/lib/QEKF/rtGetNaN.h new file mode 100644 index 0000000..b5555fc --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/rtGetNaN.h @@ -0,0 +1,26 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rtGetNaN.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +#ifndef RTGETNAN_H +#define RTGETNAN_H +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetNaN(void); +extern real32_T rtGetNaNF(void); + +#endif + +/* + * File trailer for rtGetNaN.h + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/rt_defines.h b/Estimators/QEKF/codegen/lib/QEKF/rt_defines.h new file mode 100644 index 0000000..8d86823 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/rt_defines.h @@ -0,0 +1,27 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: rt_defines.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:33:55 +// +#ifndef RT_DEFINES_H +#define RT_DEFINES_H +#include +#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 + +// +// File trailer for rt_defines.h +// +// [EOF] +// diff --git a/Estimators/QEKF/codegen/lib/QEKF/rt_nonfinite.cpp b/Estimators/QEKF/codegen/lib/QEKF/rt_nonfinite.cpp new file mode 100644 index 0000000..8742770 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/rt_nonfinite.cpp @@ -0,0 +1,83 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rt_nonfinite.cpp + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finites, + * (Inf, NaN and -Inf). + */ +#include "rt_nonfinite.h" +#include "rtGetNaN.h" +#include "rtGetInf.h" + +real_T rtInf; +real_T rtMinusInf; +real_T rtNaN; +real32_T rtInfF; +real32_T rtMinusInfF; +real32_T rtNaNF; + +/* Function: rt_InitInfAndNaN ================================================== + * Abstract: + * Initialize the rtInf, rtMinusInf, and rtNaN needed by the + * generated code. NaN is initialized as non-signaling. Assumes IEEE. + */ +void rt_InitInfAndNaN(size_t realSize) +{ + (void)realSize; + rtNaN = rtGetNaN(); + rtNaNF = rtGetNaNF(); + rtInf = rtGetInf(); + rtInfF = rtGetInfF(); + rtMinusInf = rtGetMinusInf(); + rtMinusInfF = rtGetMinusInfF(); +} + +/* Function: rtIsInf ================================================== + * Abstract: + * Test if value is infinite + */ +boolean_T rtIsInf(real_T value) +{ + return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); +} + +/* Function: rtIsInfF ================================================= + * Abstract: + * Test if single-precision value is infinite + */ +boolean_T rtIsInfF(real32_T value) +{ + return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); +} + +/* Function: rtIsNaN ================================================== + * Abstract: + * Test if value is not a number + */ +boolean_T rtIsNaN(real_T value) +{ + return (value!=value)? 1U:0U; +} + +/* Function: rtIsNaNF ================================================= + * Abstract: + * Test if single-precision value is not a number + */ +boolean_T rtIsNaNF(real32_T value) +{ + return (value!=value)? 1U:0U; +} + +/* + * File trailer for rt_nonfinite.cpp + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/rt_nonfinite.h b/Estimators/QEKF/codegen/lib/QEKF/rt_nonfinite.h new file mode 100644 index 0000000..de6059c --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/rt_nonfinite.h @@ -0,0 +1,58 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rt_nonfinite.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +#ifndef RT_NONFINITE_H +#define RT_NONFINITE_H +#if defined(_MSC_VER) && (_MSC_VER <= 1200) +#include +#endif + +#include +#include "rtwtypes.h" + +extern real_T rtInf; +extern real_T rtMinusInf; +extern real_T rtNaN; +extern real32_T rtInfF; +extern real32_T rtMinusInfF; +extern real32_T rtNaNF; +extern void rt_InitInfAndNaN(size_t realSize); +extern boolean_T rtIsInf(real_T value); +extern boolean_T rtIsInfF(real32_T value); +extern boolean_T rtIsNaN(real_T value); +extern boolean_T rtIsNaNF(real32_T value); +typedef struct { + struct { + uint32_T wordH; + uint32_T wordL; + } words; +} BigEndianIEEEDouble; + +typedef struct { + struct { + uint32_T wordL; + uint32_T wordH; + } words; +} LittleEndianIEEEDouble; + +typedef struct { + union { + real32_T wordLreal; + uint32_T wordLuint; + } wordL; +} IEEESingle; + +#endif + +/* + * File trailer for rt_nonfinite.h + * + * [EOF] + */ diff --git a/Estimators/QEKF/codegen/lib/QEKF/rtw_proj.tmw b/Estimators/QEKF/codegen/lib/QEKF/rtw_proj.tmw new file mode 100644 index 0000000..f48daf0 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/rtw_proj.tmw @@ -0,0 +1 @@ +Code generation project for QEKF using toolchain "GNU Tools for ARM Embedded Processors v5.2 | gmake (64-bit Windows)". MATLAB root = C:\Program Files\MATLAB\R2018a. diff --git a/Estimators/QEKF/codegen/lib/QEKF/rtwtypes.h b/Estimators/QEKF/codegen/lib/QEKF/rtwtypes.h new file mode 100644 index 0000000..1e6ab32 --- /dev/null +++ b/Estimators/QEKF/codegen/lib/QEKF/rtwtypes.h @@ -0,0 +1,144 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rtwtypes.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:33:55 + */ + +#ifndef RTWTYPES_H +#define RTWTYPES_H + +/*=======================================================================* + * Target hardware information + * Device type: ARM Compatible->ARM Cortex + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 + * native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * + * ulong_T, char_T and byte_T. * + *===========================================================================*/ +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef char_T byte_T; + +/*===========================================================================* + * Complex number type definitions * + *===========================================================================*/ +#define CREAL_T + +typedef struct { + real32_T re; + real32_T im; +} creal32_T; + +typedef struct { + real64_T re; + real64_T im; +} creal64_T; + +typedef struct { + real_T re; + real_T im; +} creal_T; + +typedef struct { + int8_T re; + int8_T im; +} cint8_T; + +typedef struct { + uint8_T re; + uint8_T im; +} cuint8_T; + +typedef struct { + int16_T re; + int16_T im; +} cint16_T; + +typedef struct { + uint16_T re; + uint16_T im; +} cuint16_T; + +typedef struct { + int32_T re; + int32_T im; +} cint32_T; + +typedef struct { + uint32_T re; + uint32_T im; +} cuint32_T; + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255)) +#define MIN_uint8_T ((uint8_T)(0)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535)) +#define MIN_uint16_T ((uint16_T)(0)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MIN_uint32_T ((uint32_T)(0)) + +/* Logical type definitions */ +#if !defined(__cplusplus) && !defined(__true_false_are_keywords) && !defined(__bool_true_false_are_defined) +# ifndef false +# define false (0U) +# endif + +# ifndef true +# define true (1U) +# endif +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable) + * including the null-termination character. Referenced by + * rt_logging.c and rt_matrx.c. + */ +#define TMW_NAME_LENGTH_MAX 64 +#endif + +/* + * File trailer for rtwtypes.h + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF.cpp b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF.cpp new file mode 100644 index 0000000..84f8b6e --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF.cpp @@ -0,0 +1,933 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: VelocityEKF.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// + +// Include Files +#include +#include "rt_nonfinite.h" +#include "VelocityEKF.h" +#include "mrdivide.h" + +// Function Definitions + +// +// function [X_out, P_out] = VelocityEKF(X, P_prev, EncoderDiffMeas, eta_encoder, Accelerometer, cov_acc, eta_accelerometer, eta_bias, qQEKF, cov_qQEKF, qdotQEKF, eta_acceleration, SamplePeriod, TicksPrRev, rk,rw,g) +// for q o p = Phi(q) * p +// Arguments : const float X[7] +// const float P_prev[49] +// const float EncoderDiffMeas[3] +// float eta_encoder +// const float Accelerometer[3] +// const float cov_acc[9] +// float eta_accelerometer +// float eta_bias +// const float qQEKF[4] +// const float cov_qQEKF[16] +// const float qdotQEKF[4] +// float eta_acceleration +// float SamplePeriod +// float TicksPrRev +// float rk +// float rw +// float g +// float X_out[7] +// float P_out[49] +// Return Type : void +// +void VelocityEKF(const float X[7], const float P_prev[49], const float + EncoderDiffMeas[3], float eta_encoder, const float + Accelerometer[3], const float cov_acc[9], float + eta_accelerometer, float eta_bias, const float qQEKF[4], const + float cov_qQEKF[16], const float qdotQEKF[4], float + eta_acceleration, float SamplePeriod, float TicksPrRev, float + rk, float rw, float g, float X_out[7], float P_out[49]) +{ + float y; + float b_y; + float c_y; + int i0; + float fv0[3]; + int j; + float fv1[4]; + static const float fv2[9] = { 0.0F, 1.0F, 0.0F, -0.866025388F, -0.5F, 0.0F, + 0.866025388F, -0.5F, 0.0F }; + + static const signed char iv0[3] = { 1, 0, 0 }; + + static const signed char iv1[12] = { 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + + float fv3[4]; + static const signed char iv2[3] = { 0, 1, 0 }; + + float fv4[4]; + static const signed char iv3[3] = { 0, 0, 1 }; + + float fv5[4]; + float b_qdotQEKF[4]; + float fv6[4]; + float W[12]; + static const float fv7[16] = { 0.0F, 0.353553385F, 0.612372458F, -0.707106769F, + -0.353553385F, 0.0F, 0.707106769F, 0.612372458F, -0.612372458F, + -0.707106769F, 0.0F, -0.353553385F, 0.707106769F, -0.612372458F, + 0.353553385F, 0.0F }; + + static const float fv8[16] = { 0.0F, -0.707106769F, -0.0F, -0.707106769F, + 0.707106769F, 0.0F, 0.707106769F, -0.0F, 0.0F, -0.707106769F, 0.0F, + 0.707106769F, 0.707106769F, 0.0F, -0.707106769F, 0.0F }; + + float dx_apriori; + static const float fv9[16] = { 0.0F, 0.353553385F, -0.612372458F, + -0.707106769F, -0.353553385F, 0.0F, 0.707106769F, -0.612372458F, + 0.612372458F, -0.707106769F, 0.0F, -0.353553385F, 0.707106769F, 0.612372458F, + 0.353553385F, 0.0F }; + + float dy_apriori; + float v[7]; + static float Q[49]; + static double F_prev[49]; + static const signed char iv4[49] = { 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 1 }; + + static const signed char iv5[4] = { 1, 0, 0, 1 }; + + static float b_P_prev[49]; + int i1; + static float P_apriori[49]; + float a; + static double H[42]; + float b_qQEKF[16]; + float c_qQEKF[16]; + float b_a[12]; + float daccelerometer_dqQEKF[12]; + float d_qQEKF[12]; + static const signed char iv6[4] = { 0, 0, 1, 0 }; + + static const signed char iv7[4] = { 0, -1, 0, 0 }; + + static const signed char iv8[8] = { 0, 0, 1, 0, 0, 1, 0, 0 }; + + float q[4]; + static const signed char iv9[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + float p[4]; + float d_y[16]; + float fv10[16]; + float fv11[12]; + float fv12[12]; + static const signed char iv10[16] = { 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, + 0, 0, -1 }; + + static float b_H[42]; + static float K[42]; + float c_H[36]; + float b_eta_accelerometer[9]; + float d_H[36]; + float e_y[36]; + static const float fv13[9] = { 0.5F, 0.0F, 0.0F, 0.0F, 0.5F, 0.0F, 0.0F, 0.0F, + 0.5F }; + + float fv14[3]; + float b_EncoderDiffMeas[6]; + float c_EncoderDiffMeas[6]; + float c_a[6]; + float b_dx_apriori[7]; + + // 'VelocityEKF:3' Phi = @(q)[q(1) -q(2) -q(3) -q(4); % for q o p = Phi(q) * p + // 'VelocityEKF:4' q(2) q(1) -q(4) q(3); + // 'VelocityEKF:5' q(3) q(4) q(1) -q(2); + // 'VelocityEKF:6' q(4) -q(3) q(2) q(1)]; + // for q o p = Gamma(p) * q + // 'VelocityEKF:7' Gamma = @(p)[p(1) -p(2) -p(3) -p(4); % for q o p = Gamma(p) * q + // 'VelocityEKF:8' p(2) p(1) p(4) -p(3); + // 'VelocityEKF:9' p(3) -p(4) p(1) p(2); + // 'VelocityEKF:10' p(4) p(3) -p(2) p(1)]; + // 'VelocityEKF:12' devec = [0,1,0,0;0,0,1,0;0,0,0,1]; + // 'v' in notes + // 'VelocityEKF:13' vec = [0,0,0;1,0,0;0,1,0;0,0,1]; + // '^' in notes + // 'VelocityEKF:14' I_conj = diag([1,-1,-1,-1]); + // 'VelocityEKF:16' dt = SamplePeriod; + // 'VelocityEKF:17' dx = X(1); + // 'VelocityEKF:18' dy = X(2); + // 'VelocityEKF:19' ddx = X(3); + // 'VelocityEKF:20' ddy = X(4); + // 'VelocityEKF:21' acc_bias = X(5:7); + // motor mapping (inverse kinematics) + // 'VelocityEKF:24' alpha = deg2rad(45); + // 'VelocityEKF:25' gamma = deg2rad(120); + // 'VelocityEKF:27' e1 = [1,0,0]'; + // 'VelocityEKF:28' e2 = [0,1,0]'; + // 'VelocityEKF:29' e3 = [0,0,1]'; + // 'VelocityEKF:30' R_alpha_gamma = diag([cos(alpha) cos(alpha) sin(alpha)]) * [1 cos(gamma), cos(2*gamma); 0 sin(gamma) sin(2*gamma); 1, 1, 1]; + // 'VelocityEKF:31' R_gamma = [0 -sin(gamma) -sin(2*gamma); 1 cos(gamma), cos(2*gamma); 0, 0, 0]; + // 'VelocityEKF:33' W1 = rk/rw * e1' * R_gamma' * devec * Gamma(vec * R_alpha_gamma*e1); + y = rk / rw; + + // 'VelocityEKF:34' W2 = rk/rw * e2' * R_gamma' * devec * Gamma(vec * R_alpha_gamma*e2); + b_y = rk / rw; + + // 'VelocityEKF:35' W3 = rk/rw * e3' * R_gamma' * devec * Gamma(vec * R_alpha_gamma*e3); + c_y = rk / rw; + + // 'VelocityEKF:36' W = [W1;W2;W3]; + for (i0 = 0; i0 < 3; i0++) { + fv0[i0] = 0.0F; + for (j = 0; j < 3; j++) { + fv0[i0] += fv2[i0 + 3 * j] * (y * (float)iv0[j]); + } + } + + for (i0 = 0; i0 < 4; i0++) { + fv1[i0] = 0.0F; + for (j = 0; j < 3; j++) { + fv1[i0] += (float)iv1[i0 + (j << 2)] * fv0[j]; + } + } + + for (i0 = 0; i0 < 3; i0++) { + fv0[i0] = 0.0F; + for (j = 0; j < 3; j++) { + fv0[i0] += fv2[i0 + 3 * j] * (b_y * (float)iv2[j]); + } + } + + for (i0 = 0; i0 < 4; i0++) { + fv3[i0] = 0.0F; + for (j = 0; j < 3; j++) { + fv3[i0] += (float)iv1[i0 + (j << 2)] * fv0[j]; + } + } + + for (i0 = 0; i0 < 3; i0++) { + fv0[i0] = 0.0F; + for (j = 0; j < 3; j++) { + fv0[i0] += fv2[i0 + 3 * j] * (c_y * (float)iv3[j]); + } + } + + for (i0 = 0; i0 < 4; i0++) { + fv4[i0] = 0.0F; + for (j = 0; j < 3; j++) { + fv4[i0] += (float)iv1[i0 + (j << 2)] * fv0[j]; + } + + b_qdotQEKF[i0] = 0.0F; + fv6[i0] = 0.0F; + for (j = 0; j < 4; j++) { + b_qdotQEKF[i0] += fv8[i0 + (j << 2)] * fv1[j]; + fv6[i0] += fv9[i0 + (j << 2)] * fv3[j]; + } + } + + for (i0 = 0; i0 < 4; i0++) { + fv5[i0] = 0.0F; + for (j = 0; j < 4; j++) { + fv5[i0] += fv7[i0 + (j << 2)] * fv4[j]; + } + + W[i0] = b_qdotQEKF[i0]; + W[4 + i0] = fv6[i0]; + W[8 + i0] = fv5[i0]; + } + + // %% Prediction step + // 'VelocityEKF:39' X_apriori = zeros(7,1); + // Rotate acceleration measurement from body frame into inertial frame + // 'VelocityEKF:42' acceleration = devec * Phi(qQEKF) * Gamma(qQEKF)' * [0;Accelerometer]; + // Propagate the velocity based on acceleration + // 'VelocityEKF:45' dx_apriori = dx + dt * ddx; + dx_apriori = X[0] + SamplePeriod * X[2]; + + // 'VelocityEKF:46' dy_apriori = dy + dt * ddy; + dy_apriori = X[1] + SamplePeriod * X[3]; + + // 'VelocityEKF:48' ddx_apriori = ddx; + // 'VelocityEKF:49' ddy_apriori = ddy; + // 'VelocityEKF:51' acc_bias_apriori = acc_bias; + // Setup process covariance + // 'VelocityEKF:54' Q = diag([zeros(1,2), eta_acceleration*ones(1,2), eta_bias*ones(1,3)]); + for (i0 = 0; i0 < 2; i0++) { + v[i0] = 0.0F; + v[i0 + 2] = eta_acceleration; + } + + for (i0 = 0; i0 < 3; i0++) { + v[i0 + 4] = eta_bias; + } + + memset(&Q[0], 0, 49U * sizeof(float)); + for (j = 0; j < 7; j++) { + Q[j + 7 * j] = v[j]; + } + + // Determine model Jacobian (F) + // 'VelocityEKF:57' F_prev = eye(7); + for (i0 = 0; i0 < 49; i0++) { + F_prev[i0] = iv4[i0]; + } + + // 'VelocityEKF:58' F_prev(1:2,3:4) = dt * eye(2); + for (i0 = 0; i0 < 2; i0++) { + for (j = 0; j < 2; j++) { + F_prev[(j + 7 * i0) + 2] = SamplePeriod * (float)iv5[j + (i0 << 1)]; + } + } + + // Set apriori state + // 'VelocityEKF:61' X_apriori = [dx_apriori + // 'VelocityEKF:62' dy_apriori; + // 'VelocityEKF:63' ddx_apriori; + // 'VelocityEKF:64' ddy_apriori; + // 'VelocityEKF:65' acc_bias_apriori]; + // Calculate apriori covariance of estimate error + // 'VelocityEKF:68' P_apriori = F_prev * P_prev * F_prev' + Q; + for (i0 = 0; i0 < 7; i0++) { + for (j = 0; j < 7; j++) { + b_P_prev[i0 + 7 * j] = 0.0F; + for (i1 = 0; i1 < 7; i1++) { + b_P_prev[i0 + 7 * j] += P_prev[i0 + 7 * i1] * (float)F_prev[i1 + 7 * j]; + } + } + } + + for (i0 = 0; i0 < 7; i0++) { + for (j = 0; j < 7; j++) { + c_y = 0.0F; + for (i1 = 0; i1 < 7; i1++) { + c_y += (float)F_prev[i1 + 7 * i0] * b_P_prev[i1 + 7 * j]; + } + + P_apriori[i0 + 7 * j] = c_y + Q[i0 + 7 * j]; + } + } + + // %% Update/correction step + // 'VelocityEKF:71' z = [EncoderDiffMeas; Accelerometer]; + // Encoder Measurement model + // 'VelocityEKF:74' dx_ball_apriori = dx_apriori; + // 'VelocityEKF:75' dy_ball_apriori = dy_apriori; + // 'VelocityEKF:77' dpsi_apriori = W * (1/rk * Phi(qQEKF)' * Gamma(qQEKF) * [0;-dy_ball_apriori;dx_ball_apriori;0] - 2*Phi(qQEKF)'*qdotQEKF); + y = 1.0F / rk; + + // InverseKinematics(qdotQEKF(1),qdotQEKF(2),qdotQEKF(3),qdotQEKF(4),dx_ball_apriori,dy_ball_apriori,qQEKF(1),qQEKF(2),qQEKF(3),qQEKF(4),rk,rw); + // 'VelocityEKF:78' z_encoder_hat = TicksPrRev/(2*pi) * dt * dpsi_apriori; + a = TicksPrRev / 6.28318548F * SamplePeriod; + + // Accelerometer measurement model + // 'VelocityEKF:81' z_accelerometer_hat = devec * Phi(qQEKF)' * Gamma(qQEKF) * [0;ddx_apriori;ddy_apriori;g] + acc_bias_apriori; + // Rotate acceleration from inertial frame into body frame + // Assemble measurement prediction vector + // 'VelocityEKF:84' z_hat = [z_encoder_hat; z_accelerometer_hat]; + // Measurement Jacobian + // 'VelocityEKF:87' H = zeros(6,7); + memset(&H[0], 0, 42U * sizeof(double)); + + // 'VelocityEKF:88' H(1:3,1) = TicksPrRev/(2*pi) * dt * W * 1/rk * Phi(qQEKF)' * Gamma(qQEKF) * [0;0;1;0]; + c_y = TicksPrRev / 6.28318548F * SamplePeriod; + b_qQEKF[0] = qQEKF[0]; + b_qQEKF[4] = -qQEKF[1]; + b_qQEKF[8] = -qQEKF[2]; + b_qQEKF[12] = -qQEKF[3]; + b_qQEKF[1] = qQEKF[1]; + b_qQEKF[5] = qQEKF[0]; + b_qQEKF[9] = -qQEKF[3]; + b_qQEKF[13] = qQEKF[2]; + b_qQEKF[2] = qQEKF[2]; + b_qQEKF[6] = qQEKF[3]; + b_qQEKF[10] = qQEKF[0]; + b_qQEKF[14] = -qQEKF[1]; + b_qQEKF[3] = qQEKF[3]; + b_qQEKF[7] = -qQEKF[2]; + b_qQEKF[11] = qQEKF[1]; + b_qQEKF[15] = qQEKF[0]; + for (i0 = 0; i0 < 3; i0++) { + for (j = 0; j < 4; j++) { + b_a[j + (i0 << 2)] = c_y * W[j + (i0 << 2)] / rk; + } + } + + c_qQEKF[0] = qQEKF[0]; + c_qQEKF[1] = -qQEKF[1]; + c_qQEKF[2] = -qQEKF[2]; + c_qQEKF[3] = -qQEKF[3]; + c_qQEKF[4] = qQEKF[1]; + c_qQEKF[5] = qQEKF[0]; + c_qQEKF[6] = qQEKF[3]; + c_qQEKF[7] = -qQEKF[2]; + c_qQEKF[8] = qQEKF[2]; + c_qQEKF[9] = -qQEKF[3]; + c_qQEKF[10] = qQEKF[0]; + c_qQEKF[11] = qQEKF[1]; + c_qQEKF[12] = qQEKF[3]; + c_qQEKF[13] = qQEKF[2]; + c_qQEKF[14] = -qQEKF[1]; + c_qQEKF[15] = qQEKF[0]; + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + daccelerometer_dqQEKF[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + daccelerometer_dqQEKF[i0 + (j << 2)] += b_qQEKF[i0 + (i1 << 2)] * b_a[i1 + + (j << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + d_qQEKF[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + d_qQEKF[i0 + (j << 2)] += c_qQEKF[i0 + (i1 << 2)] * + daccelerometer_dqQEKF[i1 + (j << 2)]; + } + } + } + + // d encoder_meas / d dx_2L + // 'VelocityEKF:89' H(1:3,2) = TicksPrRev/(2*pi) * dt * W * 1/rk * Phi(qQEKF)' * Gamma(qQEKF) * [0;-1;0;0]; + c_y = TicksPrRev / 6.28318548F * SamplePeriod; + b_qQEKF[0] = qQEKF[0]; + b_qQEKF[4] = -qQEKF[1]; + b_qQEKF[8] = -qQEKF[2]; + b_qQEKF[12] = -qQEKF[3]; + b_qQEKF[1] = qQEKF[1]; + b_qQEKF[5] = qQEKF[0]; + b_qQEKF[9] = -qQEKF[3]; + b_qQEKF[13] = qQEKF[2]; + b_qQEKF[2] = qQEKF[2]; + b_qQEKF[6] = qQEKF[3]; + b_qQEKF[10] = qQEKF[0]; + b_qQEKF[14] = -qQEKF[1]; + b_qQEKF[3] = qQEKF[3]; + b_qQEKF[7] = -qQEKF[2]; + b_qQEKF[11] = qQEKF[1]; + b_qQEKF[15] = qQEKF[0]; + for (i0 = 0; i0 < 3; i0++) { + fv0[i0] = 0.0F; + for (j = 0; j < 4; j++) { + b_a[j + (i0 << 2)] = c_y * W[j + (i0 << 2)] / rk; + fv0[i0] += (float)iv6[j] * d_qQEKF[j + (i0 << 2)]; + } + + H[7 * i0] = fv0[i0]; + } + + c_qQEKF[0] = qQEKF[0]; + c_qQEKF[1] = -qQEKF[1]; + c_qQEKF[2] = -qQEKF[2]; + c_qQEKF[3] = -qQEKF[3]; + c_qQEKF[4] = qQEKF[1]; + c_qQEKF[5] = qQEKF[0]; + c_qQEKF[6] = qQEKF[3]; + c_qQEKF[7] = -qQEKF[2]; + c_qQEKF[8] = qQEKF[2]; + c_qQEKF[9] = -qQEKF[3]; + c_qQEKF[10] = qQEKF[0]; + c_qQEKF[11] = qQEKF[1]; + c_qQEKF[12] = qQEKF[3]; + c_qQEKF[13] = qQEKF[2]; + c_qQEKF[14] = -qQEKF[1]; + c_qQEKF[15] = qQEKF[0]; + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + daccelerometer_dqQEKF[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + daccelerometer_dqQEKF[i0 + (j << 2)] += b_qQEKF[i0 + (i1 << 2)] * b_a[i1 + + (j << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + d_qQEKF[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + d_qQEKF[i0 + (j << 2)] += c_qQEKF[i0 + (i1 << 2)] * + daccelerometer_dqQEKF[i1 + (j << 2)]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + fv0[i0] = 0.0F; + for (j = 0; j < 4; j++) { + fv0[i0] += (float)iv7[j] * d_qQEKF[j + (i0 << 2)]; + } + + H[1 + 7 * i0] = fv0[i0]; + } + + // d encoder_meas / d dy_2L + // 'VelocityEKF:90' H(4:6,3:4) = devec * Phi(qQEKF)' * Gamma(qQEKF) * [zeros(1,2);eye(2);zeros(1,2)]; + b_qQEKF[0] = qQEKF[0]; + b_qQEKF[4] = -qQEKF[1]; + b_qQEKF[8] = -qQEKF[2]; + b_qQEKF[12] = -qQEKF[3]; + b_qQEKF[1] = qQEKF[1]; + b_qQEKF[5] = qQEKF[0]; + b_qQEKF[9] = -qQEKF[3]; + b_qQEKF[13] = qQEKF[2]; + b_qQEKF[2] = qQEKF[2]; + b_qQEKF[6] = qQEKF[3]; + b_qQEKF[10] = qQEKF[0]; + b_qQEKF[14] = -qQEKF[1]; + b_qQEKF[3] = qQEKF[3]; + b_qQEKF[7] = -qQEKF[2]; + b_qQEKF[11] = qQEKF[1]; + b_qQEKF[15] = qQEKF[0]; + c_qQEKF[0] = qQEKF[0]; + c_qQEKF[1] = -qQEKF[1]; + c_qQEKF[2] = -qQEKF[2]; + c_qQEKF[3] = -qQEKF[3]; + c_qQEKF[4] = qQEKF[1]; + c_qQEKF[5] = qQEKF[0]; + c_qQEKF[6] = qQEKF[3]; + c_qQEKF[7] = -qQEKF[2]; + c_qQEKF[8] = qQEKF[2]; + c_qQEKF[9] = -qQEKF[3]; + c_qQEKF[10] = qQEKF[0]; + c_qQEKF[11] = qQEKF[1]; + c_qQEKF[12] = qQEKF[3]; + c_qQEKF[13] = qQEKF[2]; + c_qQEKF[14] = -qQEKF[1]; + c_qQEKF[15] = qQEKF[0]; + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + daccelerometer_dqQEKF[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + daccelerometer_dqQEKF[i0 + (j << 2)] += b_qQEKF[i0 + (i1 << 2)] * (float) + iv1[i1 + (j << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + d_qQEKF[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + d_qQEKF[i0 + (j << 2)] += c_qQEKF[i0 + (i1 << 2)] * + daccelerometer_dqQEKF[i1 + (j << 2)]; + } + } + } + + for (i0 = 0; i0 < 2; i0++) { + for (j = 0; j < 3; j++) { + c_y = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + c_y += (float)iv8[i0 + (i1 << 1)] * d_qQEKF[i1 + (j << 2)]; + } + + H[(i0 + 7 * (3 + j)) + 2] = c_y; + } + } + + // 'VelocityEKF:91' H(4:6,5:7) = eye(3); + // Measurement covariances + // 'VelocityEKF:94' cov_quantization = 0.5 * eye(3); + // 'VelocityEKF:95' cov_encoder = eta_encoder * 4*cov_quantization; + b_y = eta_encoder * 4.0F; + + // 'VelocityEKF:97' daccelerometer_dqQEKF = devec * Phi(qQEKF)' * Phi([0;Accelerometer]) + devec * Gamma(qQEKF) * Gamma([0;Accelerometer]) * I_conj; + for (j = 0; j < 3; j++) { + for (i0 = 0; i0 < 3; i0++) { + H[(i0 + 7 * (3 + j)) + 4] = iv9[i0 + 3 * j]; + } + + q[j + 1] = Accelerometer[j]; + p[j + 1] = Accelerometer[j]; + } + + b_qQEKF[0] = qQEKF[0]; + b_qQEKF[4] = -qQEKF[1]; + b_qQEKF[8] = -qQEKF[2]; + b_qQEKF[12] = -qQEKF[3]; + b_qQEKF[1] = qQEKF[1]; + b_qQEKF[5] = qQEKF[0]; + b_qQEKF[9] = -qQEKF[3]; + b_qQEKF[13] = qQEKF[2]; + b_qQEKF[2] = qQEKF[2]; + b_qQEKF[6] = qQEKF[3]; + b_qQEKF[10] = qQEKF[0]; + b_qQEKF[14] = -qQEKF[1]; + b_qQEKF[3] = qQEKF[3]; + b_qQEKF[7] = -qQEKF[2]; + b_qQEKF[11] = qQEKF[1]; + b_qQEKF[15] = qQEKF[0]; + d_y[0] = 0.0F; + d_y[1] = -q[1]; + d_y[2] = -q[2]; + d_y[3] = -q[3]; + d_y[4] = q[1]; + d_y[5] = 0.0F; + d_y[6] = -q[3]; + d_y[7] = q[2]; + d_y[8] = q[2]; + d_y[9] = q[3]; + d_y[10] = 0.0F; + d_y[11] = -q[1]; + d_y[12] = q[3]; + d_y[13] = -q[2]; + d_y[14] = q[1]; + d_y[15] = 0.0F; + c_qQEKF[0] = qQEKF[0]; + c_qQEKF[1] = -qQEKF[1]; + c_qQEKF[2] = -qQEKF[2]; + c_qQEKF[3] = -qQEKF[3]; + c_qQEKF[4] = qQEKF[1]; + c_qQEKF[5] = qQEKF[0]; + c_qQEKF[6] = qQEKF[3]; + c_qQEKF[7] = -qQEKF[2]; + c_qQEKF[8] = qQEKF[2]; + c_qQEKF[9] = -qQEKF[3]; + c_qQEKF[10] = qQEKF[0]; + c_qQEKF[11] = qQEKF[1]; + c_qQEKF[12] = qQEKF[3]; + c_qQEKF[13] = qQEKF[2]; + c_qQEKF[14] = -qQEKF[1]; + c_qQEKF[15] = qQEKF[0]; + fv10[0] = 0.0F; + fv10[1] = -p[1]; + fv10[2] = -p[2]; + fv10[3] = -p[3]; + fv10[4] = p[1]; + fv10[5] = 0.0F; + fv10[6] = p[3]; + fv10[7] = -p[2]; + fv10[8] = p[2]; + fv10[9] = -p[3]; + fv10[10] = 0.0F; + fv10[11] = p[1]; + fv10[12] = p[3]; + fv10[13] = p[2]; + fv10[14] = -p[1]; + fv10[15] = 0.0F; + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + daccelerometer_dqQEKF[i0 + (j << 2)] = 0.0F; + d_qQEKF[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + daccelerometer_dqQEKF[i0 + (j << 2)] += b_qQEKF[i0 + (i1 << 2)] * (float) + iv1[i1 + (j << 2)]; + d_qQEKF[i0 + (j << 2)] += c_qQEKF[i0 + (i1 << 2)] * (float)iv1[i1 + (j << + 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + fv11[i0 + (j << 2)] = 0.0F; + fv12[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + fv11[i0 + (j << 2)] += fv10[i0 + (i1 << 2)] * d_qQEKF[i1 + (j << 2)]; + fv12[i0 + (j << 2)] += d_y[i0 + (i1 << 2)] * daccelerometer_dqQEKF[i1 + + (j << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + b_a[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + b_a[i0 + (j << 2)] += (float)iv10[i0 + (i1 << 2)] * fv11[i1 + (j << 2)]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + for (j = 0; j < 4; j++) { + daccelerometer_dqQEKF[j + (i0 << 2)] = fv12[j + (i0 << 2)] + b_a[j + (i0 << + 2)]; + } + } + + // 'VelocityEKF:98' R_accelerometer = eta_accelerometer*cov_acc + daccelerometer_dqQEKF * cov_qQEKF * daccelerometer_dqQEKF'; + // Setup measurement covariance + // 'VelocityEKF:101' R = [cov_encoder, zeros(3,3); + // 'VelocityEKF:102' zeros(3,3), R_accelerometer]; + // Calculate Kalman gain + // 'VelocityEKF:105' S = H * P_apriori * H' + R; + // K = P_apriori * H' * inv(S); + // 'VelocityEKF:107' K = P_apriori * H' / S; + for (i0 = 0; i0 < 6; i0++) { + for (j = 0; j < 7; j++) { + b_H[i0 + 6 * j] = 0.0F; + for (i1 = 0; i1 < 7; i1++) { + b_H[i0 + 6 * j] += (float)H[i1 + 7 * i0] * P_apriori[i1 + 7 * j]; + } + } + } + + for (i0 = 0; i0 < 7; i0++) { + for (j = 0; j < 6; j++) { + K[i0 + 7 * j] = 0.0F; + for (i1 = 0; i1 < 7; i1++) { + K[i0 + 7 * j] += P_apriori[i0 + 7 * i1] * (float)H[i1 + 7 * j]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + b_a[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + b_a[i0 + (j << 2)] += cov_qQEKF[i0 + (i1 << 2)] * + daccelerometer_dqQEKF[i1 + (j << 2)]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + for (j = 0; j < 3; j++) { + c_y = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + c_y += daccelerometer_dqQEKF[i1 + (i0 << 2)] * b_a[i1 + (j << 2)]; + } + + b_eta_accelerometer[i0 + 3 * j] = eta_accelerometer * cov_acc[i0 + 3 * j] + + c_y; + } + } + + for (i0 = 0; i0 < 6; i0++) { + for (j = 0; j < 6; j++) { + c_H[i0 + 6 * j] = 0.0F; + for (i1 = 0; i1 < 7; i1++) { + c_H[i0 + 6 * j] += (float)H[i1 + 7 * i0] * K[i1 + 7 * j]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + for (j = 0; j < 3; j++) { + e_y[j + 6 * i0] = b_y * fv13[j + 3 * i0]; + e_y[(j + 6 * i0) + 3] = 0.0F; + e_y[j + 6 * (i0 + 3)] = 0.0F; + e_y[(j + 6 * (i0 + 3)) + 3] = b_eta_accelerometer[j + 3 * i0]; + } + } + + for (i0 = 0; i0 < 6; i0++) { + for (j = 0; j < 6; j++) { + d_H[j + 6 * i0] = c_H[j + 6 * i0] + e_y[j + 6 * i0]; + } + } + + mrdivide(b_H, d_H, K); + + // Correct using innovation + // 'VelocityEKF:110' X_aposteriori = X_apriori + K * (z - z_hat); + fv1[0] = 0.0F; + fv1[1] = -dy_apriori; + fv1[2] = dx_apriori; + fv1[3] = 0.0F; + b_qQEKF[0] = qQEKF[0]; + b_qQEKF[1] = -qQEKF[1]; + b_qQEKF[2] = -qQEKF[2]; + b_qQEKF[3] = -qQEKF[3]; + b_qQEKF[4] = qQEKF[1]; + b_qQEKF[5] = qQEKF[0]; + b_qQEKF[6] = qQEKF[3]; + b_qQEKF[7] = -qQEKF[2]; + b_qQEKF[8] = qQEKF[2]; + b_qQEKF[9] = -qQEKF[3]; + b_qQEKF[10] = qQEKF[0]; + b_qQEKF[11] = qQEKF[1]; + b_qQEKF[12] = qQEKF[3]; + b_qQEKF[13] = qQEKF[2]; + b_qQEKF[14] = -qQEKF[1]; + b_qQEKF[15] = qQEKF[0]; + d_y[0] = y * qQEKF[0]; + d_y[4] = y * -qQEKF[1]; + d_y[8] = y * -qQEKF[2]; + d_y[12] = y * -qQEKF[3]; + d_y[1] = y * qQEKF[1]; + d_y[5] = y * qQEKF[0]; + d_y[9] = y * -qQEKF[3]; + d_y[13] = y * qQEKF[2]; + d_y[2] = y * qQEKF[2]; + d_y[6] = y * qQEKF[3]; + d_y[10] = y * qQEKF[0]; + d_y[14] = y * -qQEKF[1]; + d_y[3] = y * qQEKF[3]; + d_y[7] = y * -qQEKF[2]; + d_y[11] = y * qQEKF[1]; + d_y[15] = y * qQEKF[0]; + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 4; j++) { + c_qQEKF[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + c_qQEKF[i0 + (j << 2)] += b_qQEKF[i0 + (i1 << 2)] * d_y[i1 + (j << 2)]; + } + } + } + + d_y[0] = 2.0F * qQEKF[0]; + d_y[4] = 2.0F * -qQEKF[1]; + d_y[8] = 2.0F * -qQEKF[2]; + d_y[12] = 2.0F * -qQEKF[3]; + d_y[1] = 2.0F * qQEKF[1]; + d_y[5] = 2.0F * qQEKF[0]; + d_y[9] = 2.0F * -qQEKF[3]; + d_y[13] = 2.0F * qQEKF[2]; + d_y[2] = 2.0F * qQEKF[2]; + d_y[6] = 2.0F * qQEKF[3]; + d_y[10] = 2.0F * qQEKF[0]; + d_y[14] = 2.0F * -qQEKF[1]; + d_y[3] = 2.0F * qQEKF[3]; + d_y[7] = 2.0F * -qQEKF[2]; + d_y[11] = 2.0F * qQEKF[1]; + d_y[15] = 2.0F * qQEKF[0]; + for (i0 = 0; i0 < 4; i0++) { + fv3[i0] = 0.0F; + b_qdotQEKF[i0] = 0.0F; + for (j = 0; j < 4; j++) { + fv3[i0] += fv1[j] * c_qQEKF[j + (i0 << 2)]; + b_qdotQEKF[i0] += qdotQEKF[j] * d_y[j + (i0 << 2)]; + } + + fv4[i0] = fv3[i0] - b_qdotQEKF[i0]; + } + + for (i0 = 0; i0 < 3; i0++) { + fv0[i0] = 0.0F; + for (j = 0; j < 4; j++) { + fv0[i0] += fv4[j] * W[j + (i0 << 2)]; + } + } + + fv1[0] = 0.0F; + fv1[1] = X[2]; + fv1[2] = X[3]; + fv1[3] = g; + b_qQEKF[0] = qQEKF[0]; + b_qQEKF[4] = -qQEKF[1]; + b_qQEKF[8] = -qQEKF[2]; + b_qQEKF[12] = -qQEKF[3]; + b_qQEKF[1] = qQEKF[1]; + b_qQEKF[5] = qQEKF[0]; + b_qQEKF[9] = -qQEKF[3]; + b_qQEKF[13] = qQEKF[2]; + b_qQEKF[2] = qQEKF[2]; + b_qQEKF[6] = qQEKF[3]; + b_qQEKF[10] = qQEKF[0]; + b_qQEKF[14] = -qQEKF[1]; + b_qQEKF[3] = qQEKF[3]; + b_qQEKF[7] = -qQEKF[2]; + b_qQEKF[11] = qQEKF[1]; + b_qQEKF[15] = qQEKF[0]; + c_qQEKF[0] = qQEKF[0]; + c_qQEKF[1] = -qQEKF[1]; + c_qQEKF[2] = -qQEKF[2]; + c_qQEKF[3] = -qQEKF[3]; + c_qQEKF[4] = qQEKF[1]; + c_qQEKF[5] = qQEKF[0]; + c_qQEKF[6] = qQEKF[3]; + c_qQEKF[7] = -qQEKF[2]; + c_qQEKF[8] = qQEKF[2]; + c_qQEKF[9] = -qQEKF[3]; + c_qQEKF[10] = qQEKF[0]; + c_qQEKF[11] = qQEKF[1]; + c_qQEKF[12] = qQEKF[3]; + c_qQEKF[13] = qQEKF[2]; + c_qQEKF[14] = -qQEKF[1]; + c_qQEKF[15] = qQEKF[0]; + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + daccelerometer_dqQEKF[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + daccelerometer_dqQEKF[i0 + (j << 2)] += b_qQEKF[i0 + (i1 << 2)] * (float) + iv1[i1 + (j << 2)]; + } + } + } + + for (i0 = 0; i0 < 4; i0++) { + for (j = 0; j < 3; j++) { + d_qQEKF[i0 + (j << 2)] = 0.0F; + for (i1 = 0; i1 < 4; i1++) { + d_qQEKF[i0 + (j << 2)] += c_qQEKF[i0 + (i1 << 2)] * + daccelerometer_dqQEKF[i1 + (j << 2)]; + } + } + } + + for (i0 = 0; i0 < 3; i0++) { + fv14[i0] = 0.0F; + for (j = 0; j < 4; j++) { + fv14[i0] += fv1[j] * d_qQEKF[j + (i0 << 2)]; + } + + c_EncoderDiffMeas[i0] = EncoderDiffMeas[i0]; + c_EncoderDiffMeas[i0 + 3] = Accelerometer[i0]; + c_a[i0] = a * fv0[i0]; + c_a[i0 + 3] = fv14[i0] + X[4 + i0]; + } + + for (i0 = 0; i0 < 6; i0++) { + b_EncoderDiffMeas[i0] = c_EncoderDiffMeas[i0] - c_a[i0]; + } + + for (i0 = 0; i0 < 7; i0++) { + v[i0] = 0.0F; + for (j = 0; j < 6; j++) { + v[i0] += b_EncoderDiffMeas[j] * K[j + 6 * i0]; + } + } + + b_dx_apriori[0] = dx_apriori; + b_dx_apriori[1] = dy_apriori; + b_dx_apriori[2] = X[2]; + b_dx_apriori[3] = X[3]; + for (i0 = 0; i0 < 3; i0++) { + b_dx_apriori[i0 + 4] = X[4 + i0]; + } + + for (i0 = 0; i0 < 7; i0++) { + X_out[i0] = b_dx_apriori[i0] + v[i0]; + } + + // 'VelocityEKF:111' P_aposteriori = (eye(7) - K*H) * P_apriori; + memset(&F_prev[0], 0, 49U * sizeof(double)); + for (j = 0; j < 7; j++) { + F_prev[j + 7 * j] = 1.0; + } + + for (i0 = 0; i0 < 7; i0++) { + for (j = 0; j < 7; j++) { + c_y = 0.0F; + for (i1 = 0; i1 < 6; i1++) { + c_y += (float)H[i0 + 7 * i1] * K[i1 + 6 * j]; + } + + Q[i0 + 7 * j] = (float)F_prev[i0 + 7 * j] - c_y; + } + } + + for (i0 = 0; i0 < 7; i0++) { + for (j = 0; j < 7; j++) { + P_out[i0 + 7 * j] = 0.0F; + for (i1 = 0; i1 < 7; i1++) { + P_out[i0 + 7 * j] += P_apriori[i0 + 7 * i1] * Q[i1 + 7 * j]; + } + } + } + + // %% Send output to Simulink + // 'VelocityEKF:114' X_out = X_aposteriori; + // 'VelocityEKF:115' P_out = P_aposteriori; +} + +// +// File trailer for VelocityEKF.cpp +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF.h b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF.h new file mode 100644 index 0000000..14352cb --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF.h @@ -0,0 +1,33 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: VelocityEKF.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// +#ifndef VELOCITYEKF_H +#define VELOCITYEKF_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "VelocityEKF_types.h" + +// Function Declarations +extern void VelocityEKF(const float X[7], const float P_prev[49], const float + EncoderDiffMeas[3], float eta_encoder, const float Accelerometer[3], const + float cov_acc[9], float eta_accelerometer, float eta_bias, const float qQEKF[4], + const float cov_qQEKF[16], const float qdotQEKF[4], float eta_acceleration, + float SamplePeriod, float TicksPrRev, float rk, float rw, float g, float + X_out[7], float P_out[49]); + +#endif + +// +// File trailer for VelocityEKF.h +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_initialize.cpp b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_initialize.cpp new file mode 100644 index 0000000..9368cb2 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_initialize.cpp @@ -0,0 +1,31 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: VelocityEKF_initialize.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// + +// Include Files +#include "rt_nonfinite.h" +#include "VelocityEKF.h" +#include "VelocityEKF_initialize.h" + +// Function Definitions + +// +// Arguments : void +// Return Type : void +// +void VelocityEKF_initialize() +{ + rt_InitInfAndNaN(8U); +} + +// +// File trailer for VelocityEKF_initialize.cpp +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_initialize.h b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_initialize.h new file mode 100644 index 0000000..647201e --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_initialize.h @@ -0,0 +1,28 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: VelocityEKF_initialize.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// +#ifndef VELOCITYEKF_INITIALIZE_H +#define VELOCITYEKF_INITIALIZE_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "VelocityEKF_types.h" + +// Function Declarations +extern void VelocityEKF_initialize(); + +#endif + +// +// File trailer for VelocityEKF_initialize.h +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_ref.rsp b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_ref.rsp new file mode 100644 index 0000000..e69de29 diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_rtw.bat b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_rtw.bat new file mode 100644 index 0000000..5451a09 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_rtw.bat @@ -0,0 +1,12 @@ +@echo off + +cd . + +if "%1"=="" ("C:\PROGRA~1\MATLAB\R2018a\bin\win64\gmake" -f VelocityEKF_rtw.mk all) else ("C:\PROGRA~1\MATLAB\R2018a\bin\win64\gmake" -f VelocityEKF_rtw.mk %1) +@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/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_rtw.mk b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_rtw.mk new file mode 100644 index 0000000..848104d --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_rtw.mk @@ -0,0 +1,553 @@ +########################################################################### +## Makefile generated for MATLAB file/project 'VelocityEKF'. +## +## Makefile : VelocityEKF_rtw.mk +## Generated on : Fri Apr 26 13:40:33 2019 +## MATLAB Coder version: 4.0 (R2018a) +## +## Build Info: +## +## Final product: $(RELATIVE_PATH_TO_ANCHOR)/VelocityEKF.lib +## Product type : static-library +## +########################################################################### + +########################################################################### +## MACROS +########################################################################### + +# Macro Descriptions: +# PRODUCT_NAME Name of the system to build +# MAKEFILE Name of this makefile +# COMPUTER Computer type. See the MATLAB "computer" command. + +PRODUCT_NAME = VelocityEKF +MAKEFILE = VelocityEKF_rtw.mk +COMPUTER = PCWIN64 +MATLAB_ROOT = C:/PROGRA~1/MATLAB/R2018a +MATLAB_BIN = C:/PROGRA~1/MATLAB/R2018a/bin +MATLAB_ARCH_BIN = $(MATLAB_BIN)/win64 +MASTER_ANCHOR_DIR = +START_DIR = C:/Users/Thomas/Documents/Kugle-MATLAB/Estimators/VEKF +ARCH = win64 +RELATIVE_PATH_TO_ANCHOR = . +C_STANDARD_OPTS = +CPP_STANDARD_OPTS = + +########################################################################### +## TOOLCHAIN SPECIFICATIONS +########################################################################### + +# Toolchain Name: GNU Tools for ARM Embedded Processors v5.2 | gmake (64-bit Windows) +# Supported Version(s): +# ToolchainInfo Version: R2018a +# Specification Revision: 1.0 +# +#------------------------------------------- +# Macros assumed to be defined elsewhere +#------------------------------------------- + +# TARGET_LOAD_CMD_ARGS +# TARGET_LOAD_CMD +# MW_GNU_ARM_TOOLS_PATH +# FDATASECTIONS_FLG + +#----------- +# MACROS +#----------- + +LIBGCC = ${shell arm-none-eabi-gcc ${CFLAGS} -print-libgcc-file-name} +LIBC = ${shell arm-none-eabi-gcc ${CFLAGS} -print-file-name=libc.a} +LIBM = ${shell arm-none-eabi-gcc ${CFLAGS} -print-file-name=libm.a} +PRODUCT_BIN = $(RELATIVE_PATH_TO_ANCHOR)/$(PRODUCT_NAME).bin +PRODUCT_HEX = $(RELATIVE_PATH_TO_ANCHOR)/$(PRODUCT_NAME).hex +CPFLAGS = -O binary +SHELL = %SystemRoot%/system32/cmd.exe + +TOOLCHAIN_SRCS = +TOOLCHAIN_INCS = +TOOLCHAIN_LIBS = -lm + +#------------------------ +# BUILD TOOL COMMANDS +#------------------------ + +# Assembler: GNU ARM Assembler +AS_PATH = $(MW_GNU_ARM_TOOLS_PATH) +AS = $(AS_PATH)/arm-none-eabi-gcc + +# C Compiler: GNU ARM C Compiler +CC_PATH = $(MW_GNU_ARM_TOOLS_PATH) +CC = $(CC_PATH)/arm-none-eabi-gcc + +# Linker: GNU ARM Linker +LD_PATH = $(MW_GNU_ARM_TOOLS_PATH) +LD = $(LD_PATH)/arm-none-eabi-g++ + +# C++ Compiler: GNU ARM C++ Compiler +CPP_PATH = $(MW_GNU_ARM_TOOLS_PATH) +CPP = $(CPP_PATH)/arm-none-eabi-g++ + +# C++ Linker: GNU ARM C++ Linker +CPP_LD_PATH = $(MW_GNU_ARM_TOOLS_PATH) +CPP_LD = $(CPP_LD_PATH)/arm-none-eabi-g++ + +# Archiver: GNU ARM Archiver +AR_PATH = $(MW_GNU_ARM_TOOLS_PATH) +AR = $(AR_PATH)/arm-none-eabi-ar + +# MEX Tool: MEX Tool +MEX_PATH = $(MATLAB_ARCH_BIN) +MEX = $(MEX_PATH)/mex + +# Binary Converter: Binary Converter +OBJCOPYPATH = $(MW_GNU_ARM_TOOLS_PATH) +OBJCOPY = $(OBJCOPYPATH)/arm-none-eabi-objcopy + +# Hex Converter: Hex Converter +OBJCOPYPATH = $(MW_GNU_ARM_TOOLS_PATH) +OBJCOPY = $(OBJCOPYPATH)/arm-none-eabi-objcopy + +# Executable Size: Executable Size +EXESIZEPATH = $(MW_GNU_ARM_TOOLS_PATH) +EXESIZE = $(EXESIZEPATH)/arm-none-eabi-size + +# Download: Download +DOWNLOAD = + +# Execute: Execute +EXECUTE = $(PRODUCT) + +# Builder: GMAKE Utility +MAKE_PATH = %MATLAB%\bin\win64 +MAKE = $(MAKE_PATH)/gmake + + +#------------------------- +# Directives/Utilities +#------------------------- + +ASDEBUG = -g +AS_OUTPUT_FLAG = -o +CDEBUG = -g +C_OUTPUT_FLAG = -o +LDDEBUG = -g +OUTPUT_FLAG = -o +CPPDEBUG = -g +CPP_OUTPUT_FLAG = -o +CPPLDDEBUG = -g +OUTPUT_FLAG = -o +ARDEBUG = +STATICLIB_OUTPUT_FLAG = +MEX_DEBUG = -g +RM = @del /f/q +ECHO = @echo +MV = @move +RUN = + +#---------------------------------------- +# "Faster Builds" Build Configuration +#---------------------------------------- + +ARFLAGS = ruvs +ASFLAGS = -MMD -MP -MF"$(@:%.o=%.dep)" -MT"$@" \ + -Wall \ + -x assembler-with-cpp \ + $(ASFLAGS_ADDITIONAL) \ + $(DEFINES) \ + $(INCLUDES) \ + -c +OBJCOPYFLAGS_BIN = -O binary $(PRODUCT) $(PRODUCT_BIN) +CFLAGS = $(FDATASECTIONS_FLG) \ + -Wall \ + -MMD -MP -MF"$(@:%.o=%.dep)" -MT"$@" \ + -c \ + -O0 +CPPFLAGS = -std=c++98 \ + -fno-rtti \ + -fno-exceptions \ + $(FDATASECTIONS_FLG) \ + -Wall \ + -MMD -MP -MF"$(@:%.o=%.dep)" -MT"$@" \ + -c \ + -O0 +CPP_LDFLAGS = -Wl,--gc-sections \ + -Wl,-Map="$(PRODUCT_NAME).map" +CPP_SHAREDLIB_LDFLAGS = +DOWNLOAD_FLAGS = +EXESIZE_FLAGS = $(PRODUCT) +EXECUTE_FLAGS = +OBJCOPYFLAGS_HEX = -O ihex $(PRODUCT) $(PRODUCT_HEX) +LDFLAGS = -Wl,--gc-sections \ + -Wl,-Map="$(PRODUCT_NAME).map" +MEX_CPPFLAGS = +MEX_CPPLDFLAGS = +MEX_CFLAGS = +MEX_LDFLAGS = +MAKE_FLAGS = -f $(MAKEFILE) +SHAREDLIB_LDFLAGS = + +#-------------------- +# File extensions +#-------------------- + +ASM_Type1_Ext = .S +DEP_EXT = .dep +OBJ_EXT = .o +ASM_EXT = .s +DEP_EXT = .dep +H_EXT = .h +OBJ_EXT = .o +C_EXT = .c +EXE_EXT = .elf +SHAREDLIB_EXT = .so +CXX_EXT = .cxx +DEP_EXT = .dep +HPP_EXT = .hpp +OBJ_EXT = .o +CPP_EXT = .cpp +UNIX_TYPE1_EXT = .cc +UNIX_TYPE2_EXT = .C +EXE_EXT = .elf +SHAREDLIB_EXT = .so +STATICLIB_EXT = .lib +MEX_EXT = .mexw64 +MAKE_EXT = .mk + + +########################################################################### +## OUTPUT INFO +########################################################################### + +PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)/VelocityEKF.lib +PRODUCT_TYPE = "static-library" +BUILD_TYPE = "Static Library" + +########################################################################### +## INCLUDE PATHS +########################################################################### + +INCLUDES_BUILDINFO = -I$(START_DIR)/codegen/lib/VelocityEKF -I$(START_DIR) -I$(MATLAB_ROOT)/extern/include -I$(MATLAB_ROOT)/simulink/include -I$(MATLAB_ROOT)/rtw/c/src -I$(MATLAB_ROOT)/rtw/c/src/ext_mode/common -I$(MATLAB_ROOT)/rtw/c/ert + +INCLUDES = $(INCLUDES_BUILDINFO) + +########################################################################### +## DEFINES +########################################################################### + +DEFINES_STANDARD = -DMODEL=VelocityEKF -DHAVESTDIO -DUSE_RTMODEL + +DEFINES = $(DEFINES_STANDARD) + +########################################################################### +## SOURCE FILES +########################################################################### + +SRCS = $(START_DIR)/codegen/lib/VelocityEKF/VelocityEKF_initialize.cpp $(START_DIR)/codegen/lib/VelocityEKF/VelocityEKF_terminate.cpp $(START_DIR)/codegen/lib/VelocityEKF/VelocityEKF.cpp $(START_DIR)/codegen/lib/VelocityEKF/mrdivide.cpp $(START_DIR)/codegen/lib/VelocityEKF/rt_nonfinite.cpp $(START_DIR)/codegen/lib/VelocityEKF/rtGetNaN.cpp $(START_DIR)/codegen/lib/VelocityEKF/rtGetInf.cpp + +ALL_SRCS = $(SRCS) + +########################################################################### +## OBJECTS +########################################################################### + +OBJS = VelocityEKF_initialize.o VelocityEKF_terminate.o VelocityEKF.o mrdivide.o rt_nonfinite.o rtGetNaN.o rtGetInf.o + +ALL_OBJS = $(OBJS) + +########################################################################### +## PREBUILT OBJECT FILES +########################################################################### + +PREBUILT_OBJS = + +########################################################################### +## LIBRARIES +########################################################################### + +LIBS = + +########################################################################### +## SYSTEM LIBRARIES +########################################################################### + +SYSTEM_LIBS = + +########################################################################### +## ADDITIONAL TOOLCHAIN FLAGS +########################################################################### + +#--------------- +# C Compiler +#--------------- + +CFLAGS_BASIC = $(DEFINES) $(INCLUDES) + +CFLAGS += $(CFLAGS_BASIC) + +#----------------- +# C++ Compiler +#----------------- + +CPPFLAGS_BASIC = $(DEFINES) $(INCLUDES) + +CPPFLAGS += $(CPPFLAGS_BASIC) + +########################################################################### +## INLINED COMMANDS +########################################################################### + + +ALL_DEPS:=$(patsubst %$(OBJ_EXT),%$(DEP_EXT),$(ALL_OBJS)) +all: + +ifndef DISABLE_GCC_FUNCTION_DATA_SECTIONS +FDATASECTIONS_FLG := -ffunction-sections -fdata-sections +endif + + + +-include codertarget_assembly_flags.mk +-include ../codertarget_assembly_flags.mk +-include $(ALL_DEPS) + + +########################################################################### +## PHONY TARGETS +########################################################################### + +.PHONY : all build clean info prebuild postbuild download execute + + +all : build postbuild + @echo "### Successfully generated all binary outputs." + + +build : prebuild $(PRODUCT) + + +prebuild : + + +postbuild : build + + +download : postbuild + + +execute : download + + +########################################################################### +## FINAL TARGET +########################################################################### + +#--------------------------------- +# Create a static library +#--------------------------------- + +$(PRODUCT) : $(OBJS) $(PREBUILT_OBJS) + @echo "### Creating static library "$(PRODUCT)" ..." + $(AR) $(ARFLAGS) $(PRODUCT) $(OBJS) + @echo "### Created: $(PRODUCT)" + + +########################################################################### +## INTERMEDIATE TARGETS +########################################################################### + +#--------------------- +# SOURCE-TO-OBJECT +#--------------------- + +%.o : %.c + $(CC) $(CFLAGS) -o $@ $< + + +%.o : %.s + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : %.S + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : %.cpp + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : %.cc + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : %.C + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : %.cxx + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.c + $(CC) $(CFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.s + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.S + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.cpp + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.cc + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.C + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(RELATIVE_PATH_TO_ANCHOR)/%.cxx + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.c + $(CC) $(CFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.s + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.S + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.cpp + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.cc + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.C + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/%.cxx + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/VelocityEKF/%.c + $(CC) $(CFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/VelocityEKF/%.s + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/VelocityEKF/%.S + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/VelocityEKF/%.cpp + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/VelocityEKF/%.cc + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/VelocityEKF/%.C + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(START_DIR)/codegen/lib/VelocityEKF/%.cxx + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.c + $(CC) $(CFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.s + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.S + $(AS) $(ASFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.cpp + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.cc + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.C + $(CPP) $(CPPFLAGS) -o $@ $< + + +%.o : $(MATLAB_ROOT)/rtw/c/src/%.cxx + $(CPP) $(CPPFLAGS) -o $@ $< + + +########################################################################### +## DEPENDENCIES +########################################################################### + +$(ALL_OBJS) : $(MAKEFILE) rtw_proj.tmw + + +########################################################################### +## MISCELLANEOUS TARGETS +########################################################################### + +info : + @echo "### PRODUCT = $(PRODUCT)" + @echo "### PRODUCT_TYPE = $(PRODUCT_TYPE)" + @echo "### BUILD_TYPE = $(BUILD_TYPE)" + @echo "### INCLUDES = $(INCLUDES)" + @echo "### DEFINES = $(DEFINES)" + @echo "### ALL_SRCS = $(ALL_SRCS)" + @echo "### ALL_OBJS = $(ALL_OBJS)" + @echo "### LIBS = $(LIBS)" + @echo "### MODELREF_LIBS = $(MODELREF_LIBS)" + @echo "### SYSTEM_LIBS = $(SYSTEM_LIBS)" + @echo "### TOOLCHAIN_LIBS = $(TOOLCHAIN_LIBS)" + @echo "### ASFLAGS = $(ASFLAGS)" + @echo "### CFLAGS = $(CFLAGS)" + @echo "### LDFLAGS = $(LDFLAGS)" + @echo "### SHAREDLIB_LDFLAGS = $(SHAREDLIB_LDFLAGS)" + @echo "### CPPFLAGS = $(CPPFLAGS)" + @echo "### CPP_LDFLAGS = $(CPP_LDFLAGS)" + @echo "### CPP_SHAREDLIB_LDFLAGS = $(CPP_SHAREDLIB_LDFLAGS)" + @echo "### ARFLAGS = $(ARFLAGS)" + @echo "### MEX_CFLAGS = $(MEX_CFLAGS)" + @echo "### MEX_CPPFLAGS = $(MEX_CPPFLAGS)" + @echo "### MEX_LDFLAGS = $(MEX_LDFLAGS)" + @echo "### MEX_CPPLDFLAGS = $(MEX_CPPLDFLAGS)" + @echo "### OBJCOPYFLAGS_BIN = $(OBJCOPYFLAGS_BIN)" + @echo "### OBJCOPYFLAGS_HEX = $(OBJCOPYFLAGS_HEX)" + @echo "### EXESIZE_FLAGS = $(EXESIZE_FLAGS)" + @echo "### DOWNLOAD_FLAGS = $(DOWNLOAD_FLAGS)" + @echo "### EXECUTE_FLAGS = $(EXECUTE_FLAGS)" + @echo "### MAKE_FLAGS = $(MAKE_FLAGS)" + + +clean : + $(ECHO) "### Deleting all derived files..." + $(RM) $(subst /,\,$(PRODUCT)) + $(RM) $(subst /,\,$(ALL_OBJS)) + $(RM) *.dep + $(ECHO) "### Deleted all derived files." + + diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_terminate.cpp b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_terminate.cpp new file mode 100644 index 0000000..7fc0958 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_terminate.cpp @@ -0,0 +1,31 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: VelocityEKF_terminate.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// + +// Include Files +#include "rt_nonfinite.h" +#include "VelocityEKF.h" +#include "VelocityEKF_terminate.h" + +// Function Definitions + +// +// Arguments : void +// Return Type : void +// +void VelocityEKF_terminate() +{ + // (no terminate code required) +} + +// +// File trailer for VelocityEKF_terminate.cpp +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_terminate.h b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_terminate.h new file mode 100644 index 0000000..96f9145 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_terminate.h @@ -0,0 +1,28 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: VelocityEKF_terminate.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// +#ifndef VELOCITYEKF_TERMINATE_H +#define VELOCITYEKF_TERMINATE_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "VelocityEKF_types.h" + +// Function Declarations +extern void VelocityEKF_terminate(); + +#endif + +// +// File trailer for VelocityEKF_terminate.h +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_types.h b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_types.h new file mode 100644 index 0000000..dcd6293 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/VelocityEKF_types.h @@ -0,0 +1,21 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: VelocityEKF_types.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// +#ifndef VELOCITYEKF_TYPES_H +#define VELOCITYEKF_TYPES_H + +// Include Files +#include "rtwtypes.h" +#endif + +// +// File trailer for VelocityEKF_types.h +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/buildInfo.mat b/Estimators/VEKF/codegen/lib/VelocityEKF/buildInfo.mat new file mode 100644 index 0000000..438523b Binary files /dev/null and b/Estimators/VEKF/codegen/lib/VelocityEKF/buildInfo.mat differ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/codeInfo.mat b/Estimators/VEKF/codegen/lib/VelocityEKF/codeInfo.mat new file mode 100644 index 0000000..2a4fd64 Binary files /dev/null and b/Estimators/VEKF/codegen/lib/VelocityEKF/codeInfo.mat differ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/codedescriptor.dmr b/Estimators/VEKF/codegen/lib/VelocityEKF/codedescriptor.dmr new file mode 100644 index 0000000..312f990 Binary files /dev/null and b/Estimators/VEKF/codegen/lib/VelocityEKF/codedescriptor.dmr differ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/examples/main.cpp b/Estimators/VEKF/codegen/lib/VelocityEKF/examples/main.cpp new file mode 100644 index 0000000..ebdd85f --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/examples/main.cpp @@ -0,0 +1,236 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: main.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// + +//*********************************************************************** +// This automatically generated example C main file shows how to call +// entry-point functions that MATLAB Coder generated. You must customize +// this file for your application. Do not modify this file directly. +// Instead, make a copy of this file, modify it, and integrate it into +// your development environment. +// +// This file initializes entry-point function arguments to a default +// size and value before calling the entry-point functions. It does +// not store or use any values returned from the entry-point functions. +// If necessary, it does pre-allocate memory for returned values. +// You can use this file as a starting point for a main function that +// you can deploy in your application. +// +// After you copy the file, and before you deploy it, you must make the +// following changes: +// * For variable-size function arguments, change the example sizes to +// the sizes that your application requires. +// * Change the example values of function arguments to the values that +// your application requires. +// * If the entry-point functions return values, store these values or +// otherwise use them as required by your application. +// +//*********************************************************************** +// Include Files +#include "rt_nonfinite.h" +#include "VelocityEKF.h" +#include "main.h" +#include "VelocityEKF_terminate.h" +#include "VelocityEKF_initialize.h" + +// Function Declarations +static void argInit_3x1_real32_T(float result[3]); +static void argInit_3x3_real32_T(float result[9]); +static void argInit_4x1_real32_T(float result[4]); +static void argInit_4x4_real32_T(float result[16]); +static void argInit_7x1_real32_T(float result[7]); +static void argInit_7x7_real32_T(float result[49]); +static float argInit_real32_T(); +static void main_VelocityEKF(); + +// Function Definitions + +// +// Arguments : float result[3] +// Return Type : void +// +static void argInit_3x1_real32_T(float result[3]) +{ + int idx0; + + // Loop over the array to initialize each element. + for (idx0 = 0; idx0 < 3; idx0++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx0] = argInit_real32_T(); + } +} + +// +// Arguments : float result[9] +// Return Type : void +// +static void argInit_3x3_real32_T(float result[9]) +{ + int idx0; + int idx1; + + // Loop over the array to initialize each element. + for (idx0 = 0; idx0 < 3; idx0++) { + for (idx1 = 0; idx1 < 3; idx1++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx1 + 3 * idx0] = argInit_real32_T(); + } + } +} + +// +// Arguments : float result[4] +// Return Type : void +// +static void argInit_4x1_real32_T(float result[4]) +{ + int idx0; + + // Loop over the array to initialize each element. + for (idx0 = 0; idx0 < 4; idx0++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx0] = argInit_real32_T(); + } +} + +// +// Arguments : float result[16] +// Return Type : void +// +static void argInit_4x4_real32_T(float result[16]) +{ + int idx0; + int idx1; + + // Loop over the array to initialize each element. + for (idx0 = 0; idx0 < 4; idx0++) { + for (idx1 = 0; idx1 < 4; idx1++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx1 + (idx0 << 2)] = argInit_real32_T(); + } + } +} + +// +// Arguments : float result[7] +// Return Type : void +// +static void argInit_7x1_real32_T(float result[7]) +{ + int idx0; + + // Loop over the array to initialize each element. + for (idx0 = 0; idx0 < 7; idx0++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx0] = argInit_real32_T(); + } +} + +// +// Arguments : float result[49] +// Return Type : void +// +static void argInit_7x7_real32_T(float result[49]) +{ + int idx0; + int idx1; + + // Loop over the array to initialize each element. + for (idx0 = 0; idx0 < 7; idx0++) { + for (idx1 = 0; idx1 < 7; idx1++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx1 + 7 * idx0] = argInit_real32_T(); + } + } +} + +// +// Arguments : void +// Return Type : float +// +static float argInit_real32_T() +{ + return 0.0F; +} + +// +// Arguments : void +// Return Type : void +// +static void main_VelocityEKF() +{ + float X[7]; + static float P_prev[49]; + float fv15[3]; + float fv16[3]; + float fv17[9]; + float fv18[4]; + float fv19[16]; + float fv20[4]; + float X_out[7]; + static float P_out[49]; + + // Initialize function 'VelocityEKF' input arguments. + // Initialize function input argument 'X'. + argInit_7x1_real32_T(X); + + // Initialize function input argument 'P_prev'. + argInit_7x7_real32_T(P_prev); + + // Initialize function input argument 'EncoderDiffMeas'. + // Initialize function input argument 'Accelerometer'. + // Initialize function input argument 'cov_acc'. + // Initialize function input argument 'qQEKF'. + // Initialize function input argument 'cov_qQEKF'. + // Initialize function input argument 'qdotQEKF'. + // Call the entry-point 'VelocityEKF'. + argInit_3x1_real32_T(fv15); + argInit_3x1_real32_T(fv16); + argInit_3x3_real32_T(fv17); + argInit_4x1_real32_T(fv18); + argInit_4x4_real32_T(fv19); + argInit_4x1_real32_T(fv20); + VelocityEKF(X, P_prev, fv15, argInit_real32_T(), fv16, fv17, argInit_real32_T(), + argInit_real32_T(), fv18, fv19, fv20, argInit_real32_T(), + argInit_real32_T(), argInit_real32_T(), argInit_real32_T(), + argInit_real32_T(), argInit_real32_T(), X_out, P_out); +} + +// +// Arguments : int argc +// const char * const argv[] +// Return Type : int +// +int main(int, const char * const []) +{ + // Initialize the application. + // You do not need to do this more than one time. + VelocityEKF_initialize(); + + // Invoke the entry-point functions. + // You can call entry-point functions multiple times. + main_VelocityEKF(); + + // Terminate the application. + // You do not need to do this more than one time. + VelocityEKF_terminate(); + return 0; +} + +// +// File trailer for main.cpp +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/examples/main.h b/Estimators/VEKF/codegen/lib/VelocityEKF/examples/main.h new file mode 100644 index 0000000..66a632f --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/examples/main.h @@ -0,0 +1,53 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: main.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// + +//*********************************************************************** +// This automatically generated example C main file shows how to call +// entry-point functions that MATLAB Coder generated. You must customize +// this file for your application. Do not modify this file directly. +// Instead, make a copy of this file, modify it, and integrate it into +// your development environment. +// +// This file initializes entry-point function arguments to a default +// size and value before calling the entry-point functions. It does +// not store or use any values returned from the entry-point functions. +// If necessary, it does pre-allocate memory for returned values. +// You can use this file as a starting point for a main function that +// you can deploy in your application. +// +// After you copy the file, and before you deploy it, you must make the +// following changes: +// * For variable-size function arguments, change the example sizes to +// the sizes that your application requires. +// * Change the example values of function arguments to the values that +// your application requires. +// * If the entry-point functions return values, store these values or +// otherwise use them as required by your application. +// +//*********************************************************************** +#ifndef MAIN_H +#define MAIN_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "VelocityEKF_types.h" + +// Function Declarations +extern int main(int argc, const char * const argv[]); + +#endif + +// +// File trailer for main.h +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/html/report.mldatx b/Estimators/VEKF/codegen/lib/VelocityEKF/html/report.mldatx new file mode 100644 index 0000000..2fe18cf Binary files /dev/null and b/Estimators/VEKF/codegen/lib/VelocityEKF/html/report.mldatx differ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_api.c b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_api.c new file mode 100644 index 0000000..690d9f0 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_api.c @@ -0,0 +1,679 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_VelocityEKF_api.c + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +/* Include Files */ +#include "tmwtypes.h" +#include "_coder_VelocityEKF_api.h" +#include "_coder_VelocityEKF_mex.h" + +/* Variable Definitions */ +emlrtCTX emlrtRootTLSGlobal = NULL; +emlrtContext emlrtContextGlobal = { true,/* bFirstTime */ + false, /* bInitialized */ + 131466U, /* fVersionInfo */ + NULL, /* fErrorFunction */ + "VelocityEKF", /* fFunctionName */ + NULL, /* fRTCallStack */ + false, /* bDebugMode */ + { 2045744189U, 2170104910U, 2743257031U, 4284093946U },/* fSigWrd */ + NULL /* fSigMem */ +}; + +/* Function Declarations */ +static void b_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[7]); +static const mxArray *b_emlrt_marshallOut(const real32_T u[49]); +static void c_emlrt_marshallIn(const emlrtStack *sp, const mxArray *P_prev, + const char_T *identifier, real32_T y[49]); +static void d_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[49]); +static void e_emlrt_marshallIn(const emlrtStack *sp, const mxArray + *EncoderDiffMeas, const char_T *identifier, real32_T y[3]); +static void emlrt_marshallIn(const emlrtStack *sp, const mxArray *X, const + char_T *identifier, real32_T y[7]); +static const mxArray *emlrt_marshallOut(const real32_T u[7]); +static void f_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[3]); +static real32_T g_emlrt_marshallIn(const emlrtStack *sp, const mxArray + *eta_encoder, const char_T *identifier); +static real32_T h_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId); +static void i_emlrt_marshallIn(const emlrtStack *sp, const mxArray *cov_acc, + const char_T *identifier, real32_T y[9]); +static void j_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[9]); +static void k_emlrt_marshallIn(const emlrtStack *sp, const mxArray *qQEKF, const + char_T *identifier, real32_T y[4]); +static void l_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[4]); +static void m_emlrt_marshallIn(const emlrtStack *sp, const mxArray *cov_qQEKF, + const char_T *identifier, real32_T y[16]); +static void n_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[16]); +static void o_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[7]); +static void p_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[49]); +static void q_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[3]); +static real32_T r_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, + const emlrtMsgIdentifier *msgId); +static void s_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[9]); +static void t_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[4]); +static void u_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[16]); + +/* Function Definitions */ + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * real32_T y[7] + * Return Type : void + */ +static void b_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[7]) +{ + o_emlrt_marshallIn(sp, emlrtAlias(u), parentId, y); + emlrtDestroyArray(&u); +} + +/* + * Arguments : const real32_T u[49] + * Return Type : const mxArray * + */ +static const mxArray *b_emlrt_marshallOut(const real32_T u[49]) +{ + const mxArray *y; + int32_T i7; + const mxArray *m1; + static const int32_T iv1[2] = { 7, 7 }; + + int32_T i; + real32_T *pData; + real32_T fv3[49]; + int32_T b_i; + y = NULL; + for (i7 = 0; i7 < 7; i7++) { + for (i = 0; i < 7; i++) { + fv3[i + 7 * i7] = u[i7 + 7 * i]; + } + } + + m1 = emlrtCreateNumericArray(2, iv1, mxSINGLE_CLASS, mxREAL); + pData = (real32_T *)emlrtMxGetData(m1); + i7 = 0; + for (i = 0; i < 7; i++) { + for (b_i = 0; b_i < 7; b_i++) { + pData[i7] = fv3[b_i + 7 * i]; + i7++; + } + } + + emlrtAssign(&y, m1); + return y; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *P_prev + * const char_T *identifier + * real32_T y[49] + * Return Type : void + */ +static void c_emlrt_marshallIn(const emlrtStack *sp, const mxArray *P_prev, + const char_T *identifier, real32_T y[49]) +{ + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + d_emlrt_marshallIn(sp, emlrtAlias(P_prev), &thisId, y); + emlrtDestroyArray(&P_prev); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * real32_T y[49] + * Return Type : void + */ +static void d_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[49]) +{ + real32_T fv0[49]; + int32_T i0; + int32_T i1; + p_emlrt_marshallIn(sp, emlrtAlias(u), parentId, fv0); + for (i0 = 0; i0 < 7; i0++) { + for (i1 = 0; i1 < 7; i1++) { + y[i1 + 7 * i0] = fv0[i0 + 7 * i1]; + } + } + + emlrtDestroyArray(&u); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *EncoderDiffMeas + * const char_T *identifier + * real32_T y[3] + * Return Type : void + */ +static void e_emlrt_marshallIn(const emlrtStack *sp, const mxArray + *EncoderDiffMeas, const char_T *identifier, real32_T y[3]) +{ + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + f_emlrt_marshallIn(sp, emlrtAlias(EncoderDiffMeas), &thisId, y); + emlrtDestroyArray(&EncoderDiffMeas); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *X + * const char_T *identifier + * real32_T y[7] + * Return Type : void + */ +static void emlrt_marshallIn(const emlrtStack *sp, const mxArray *X, const + char_T *identifier, real32_T y[7]) +{ + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + b_emlrt_marshallIn(sp, emlrtAlias(X), &thisId, y); + emlrtDestroyArray(&X); +} + +/* + * Arguments : const real32_T u[7] + * Return Type : const mxArray * + */ +static const mxArray *emlrt_marshallOut(const real32_T u[7]) +{ + const mxArray *y; + const mxArray *m0; + static const int32_T iv0[1] = { 7 }; + + real32_T *pData; + int32_T i6; + int32_T i; + y = NULL; + m0 = emlrtCreateNumericArray(1, iv0, mxSINGLE_CLASS, mxREAL); + pData = (real32_T *)emlrtMxGetData(m0); + i6 = 0; + for (i = 0; i < 7; i++) { + pData[i6] = u[i]; + i6++; + } + + emlrtAssign(&y, m0); + return y; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * real32_T y[3] + * Return Type : void + */ +static void f_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[3]) +{ + q_emlrt_marshallIn(sp, emlrtAlias(u), parentId, y); + emlrtDestroyArray(&u); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *eta_encoder + * const char_T *identifier + * Return Type : real32_T + */ +static real32_T g_emlrt_marshallIn(const emlrtStack *sp, const mxArray + *eta_encoder, const char_T *identifier) +{ + real32_T y; + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + y = h_emlrt_marshallIn(sp, emlrtAlias(eta_encoder), &thisId); + emlrtDestroyArray(&eta_encoder); + return y; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * Return Type : real32_T + */ +static real32_T h_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId) +{ + real32_T y; + y = r_emlrt_marshallIn(sp, emlrtAlias(u), parentId); + emlrtDestroyArray(&u); + return y; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *cov_acc + * const char_T *identifier + * real32_T y[9] + * Return Type : void + */ +static void i_emlrt_marshallIn(const emlrtStack *sp, const mxArray *cov_acc, + const char_T *identifier, real32_T y[9]) +{ + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + j_emlrt_marshallIn(sp, emlrtAlias(cov_acc), &thisId, y); + emlrtDestroyArray(&cov_acc); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * real32_T y[9] + * Return Type : void + */ +static void j_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[9]) +{ + real32_T fv1[9]; + int32_T i2; + int32_T i3; + s_emlrt_marshallIn(sp, emlrtAlias(u), parentId, fv1); + for (i2 = 0; i2 < 3; i2++) { + for (i3 = 0; i3 < 3; i3++) { + y[i3 + 3 * i2] = fv1[i2 + 3 * i3]; + } + } + + emlrtDestroyArray(&u); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *qQEKF + * const char_T *identifier + * real32_T y[4] + * Return Type : void + */ +static void k_emlrt_marshallIn(const emlrtStack *sp, const mxArray *qQEKF, const + char_T *identifier, real32_T y[4]) +{ + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + l_emlrt_marshallIn(sp, emlrtAlias(qQEKF), &thisId, y); + emlrtDestroyArray(&qQEKF); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * real32_T y[4] + * Return Type : void + */ +static void l_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[4]) +{ + t_emlrt_marshallIn(sp, emlrtAlias(u), parentId, y); + emlrtDestroyArray(&u); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *cov_qQEKF + * const char_T *identifier + * real32_T y[16] + * Return Type : void + */ +static void m_emlrt_marshallIn(const emlrtStack *sp, const mxArray *cov_qQEKF, + const char_T *identifier, real32_T y[16]) +{ + emlrtMsgIdentifier thisId; + thisId.fIdentifier = (const char *)identifier; + thisId.fParent = NULL; + thisId.bParentIsCell = false; + n_emlrt_marshallIn(sp, emlrtAlias(cov_qQEKF), &thisId, y); + emlrtDestroyArray(&cov_qQEKF); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *u + * const emlrtMsgIdentifier *parentId + * real32_T y[16] + * Return Type : void + */ +static void n_emlrt_marshallIn(const emlrtStack *sp, const mxArray *u, const + emlrtMsgIdentifier *parentId, real32_T y[16]) +{ + real32_T fv2[16]; + int32_T i4; + int32_T i5; + u_emlrt_marshallIn(sp, emlrtAlias(u), parentId, fv2); + for (i4 = 0; i4 < 4; i4++) { + for (i5 = 0; i5 < 4; i5++) { + y[i5 + (i4 << 2)] = fv2[i4 + (i5 << 2)]; + } + } + + emlrtDestroyArray(&u); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * real32_T ret[7] + * Return Type : void + */ +static void o_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[7]) +{ + static const int32_T dims[1] = { 7 }; + + int32_T i8; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 1U, *(int32_T (*)[1])&dims[0]); + for (i8 = 0; i8 < 7; i8++) { + ret[i8] = (*(real32_T (*)[7])emlrtMxGetData(src))[i8]; + } + + emlrtDestroyArray(&src); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * real32_T ret[49] + * Return Type : void + */ +static void p_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[49]) +{ + static const int32_T dims[2] = { 7, 7 }; + + int32_T i9; + int32_T i10; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 2U, *(int32_T (*)[2])&dims[0]); + for (i9 = 0; i9 < 7; i9++) { + for (i10 = 0; i10 < 7; i10++) { + ret[i10 + 7 * i9] = (*(real32_T (*)[49])emlrtMxGetData(src))[i10 + 7 * i9]; + } + } + + emlrtDestroyArray(&src); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * real32_T ret[3] + * Return Type : void + */ +static void q_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[3]) +{ + static const int32_T dims[1] = { 3 }; + + int32_T i11; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 1U, *(int32_T (*)[1])&dims[0]); + for (i11 = 0; i11 < 3; i11++) { + ret[i11] = (*(real32_T (*)[3])emlrtMxGetData(src))[i11]; + } + + emlrtDestroyArray(&src); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * Return Type : real32_T + */ +static real32_T r_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, + const emlrtMsgIdentifier *msgId) +{ + real32_T ret; + static const int32_T dims = 0; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 0U, (int32_T *)&dims); + ret = *(real32_T *)emlrtMxGetData(src); + emlrtDestroyArray(&src); + return ret; +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * real32_T ret[9] + * Return Type : void + */ +static void s_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[9]) +{ + static const int32_T dims[2] = { 3, 3 }; + + int32_T i12; + int32_T i13; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 2U, *(int32_T (*)[2])&dims[0]); + for (i12 = 0; i12 < 3; i12++) { + for (i13 = 0; i13 < 3; i13++) { + ret[i13 + 3 * i12] = (*(real32_T (*)[9])emlrtMxGetData(src))[i13 + 3 * i12]; + } + } + + emlrtDestroyArray(&src); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * real32_T ret[4] + * Return Type : void + */ +static void t_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[4]) +{ + static const int32_T dims[1] = { 4 }; + + int32_T i14; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 1U, *(int32_T (*)[1])&dims[0]); + for (i14 = 0; i14 < 4; i14++) { + ret[i14] = (*(real32_T (*)[4])emlrtMxGetData(src))[i14]; + } + + emlrtDestroyArray(&src); +} + +/* + * Arguments : const emlrtStack *sp + * const mxArray *src + * const emlrtMsgIdentifier *msgId + * real32_T ret[16] + * Return Type : void + */ +static void u_emlrt_marshallIn(const emlrtStack *sp, const mxArray *src, const + emlrtMsgIdentifier *msgId, real32_T ret[16]) +{ + static const int32_T dims[2] = { 4, 4 }; + + int32_T i15; + int32_T i16; + emlrtCheckBuiltInR2012b(sp, (const emlrtMsgIdentifier *)msgId, src, "single", + false, 2U, *(int32_T (*)[2])&dims[0]); + for (i15 = 0; i15 < 4; i15++) { + for (i16 = 0; i16 < 4; i16++) { + ret[i16 + (i15 << 2)] = (*(real32_T (*)[16])emlrtMxGetData(src))[i16 + + (i15 << 2)]; + } + } + + emlrtDestroyArray(&src); +} + +/* + * Arguments : const mxArray * const prhs[17] + * int32_T nlhs + * const mxArray *plhs[2] + * Return Type : void + */ +void VelocityEKF_api(const mxArray * const prhs[17], int32_T nlhs, const mxArray + *plhs[2]) +{ + real32_T X[7]; + real32_T P_prev[49]; + real32_T EncoderDiffMeas[3]; + real32_T eta_encoder; + real32_T Accelerometer[3]; + real32_T cov_acc[9]; + real32_T eta_accelerometer; + real32_T eta_bias; + real32_T qQEKF[4]; + real32_T cov_qQEKF[16]; + real32_T qdotQEKF[4]; + real32_T eta_acceleration; + real32_T SamplePeriod; + real32_T TicksPrRev; + real32_T rk; + real32_T rw; + real32_T g; + real32_T X_out[7]; + real32_T P_out[49]; + emlrtStack st = { NULL, /* site */ + NULL, /* tls */ + NULL /* prev */ + }; + + st.tls = emlrtRootTLSGlobal; + + /* Marshall function inputs */ + emlrt_marshallIn(&st, emlrtAliasP(prhs[0]), "X", X); + c_emlrt_marshallIn(&st, emlrtAliasP(prhs[1]), "P_prev", P_prev); + e_emlrt_marshallIn(&st, emlrtAliasP(prhs[2]), "EncoderDiffMeas", + EncoderDiffMeas); + eta_encoder = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[3]), "eta_encoder"); + e_emlrt_marshallIn(&st, emlrtAliasP(prhs[4]), "Accelerometer", Accelerometer); + i_emlrt_marshallIn(&st, emlrtAliasP(prhs[5]), "cov_acc", cov_acc); + eta_accelerometer = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[6]), + "eta_accelerometer"); + eta_bias = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[7]), "eta_bias"); + k_emlrt_marshallIn(&st, emlrtAliasP(prhs[8]), "qQEKF", qQEKF); + m_emlrt_marshallIn(&st, emlrtAliasP(prhs[9]), "cov_qQEKF", cov_qQEKF); + k_emlrt_marshallIn(&st, emlrtAliasP(prhs[10]), "qdotQEKF", qdotQEKF); + eta_acceleration = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[11]), + "eta_acceleration"); + SamplePeriod = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[12]), "SamplePeriod"); + TicksPrRev = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[13]), "TicksPrRev"); + rk = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[14]), "rk"); + rw = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[15]), "rw"); + g = g_emlrt_marshallIn(&st, emlrtAliasP(prhs[16]), "g"); + + /* Invoke the target function */ + VelocityEKF(X, P_prev, EncoderDiffMeas, eta_encoder, Accelerometer, cov_acc, + eta_accelerometer, eta_bias, qQEKF, cov_qQEKF, qdotQEKF, + eta_acceleration, SamplePeriod, TicksPrRev, rk, rw, g, X_out, + P_out); + + /* Marshall function outputs */ + plhs[0] = emlrt_marshallOut(X_out); + if (nlhs > 1) { + plhs[1] = b_emlrt_marshallOut(P_out); + } +} + +/* + * Arguments : void + * Return Type : void + */ +void VelocityEKF_atexit(void) +{ + emlrtStack st = { NULL, /* site */ + NULL, /* tls */ + NULL /* prev */ + }; + + mexFunctionCreateRootTLS(); + st.tls = emlrtRootTLSGlobal; + emlrtEnterRtStackR2012b(&st); + emlrtLeaveRtStackR2012b(&st); + emlrtDestroyRootTLS(&emlrtRootTLSGlobal); + VelocityEKF_xil_terminate(); +} + +/* + * Arguments : void + * Return Type : void + */ +void VelocityEKF_initialize(void) +{ + emlrtStack st = { NULL, /* site */ + NULL, /* tls */ + NULL /* prev */ + }; + + mexFunctionCreateRootTLS(); + st.tls = emlrtRootTLSGlobal; + emlrtClearAllocCountR2012b(&st, false, 0U, 0); + emlrtEnterRtStackR2012b(&st); + emlrtFirstTimeR2012b(emlrtRootTLSGlobal); +} + +/* + * Arguments : void + * Return Type : void + */ +void VelocityEKF_terminate(void) +{ + emlrtStack st = { NULL, /* site */ + NULL, /* tls */ + NULL /* prev */ + }; + + st.tls = emlrtRootTLSGlobal; + emlrtLeaveRtStackR2012b(&st); + emlrtDestroyRootTLS(&emlrtRootTLSGlobal); +} + +/* + * File trailer for _coder_VelocityEKF_api.c + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_api.h b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_api.h new file mode 100644 index 0000000..daffd69 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_api.h @@ -0,0 +1,46 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_VelocityEKF_api.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +#ifndef _CODER_VELOCITYEKF_API_H +#define _CODER_VELOCITYEKF_API_H + +/* Include Files */ +#include "tmwtypes.h" +#include "mex.h" +#include "emlrt.h" +#include +#include +#include "_coder_VelocityEKF_api.h" + +/* Variable Declarations */ +extern emlrtCTX emlrtRootTLSGlobal; +extern emlrtContext emlrtContextGlobal; + +/* Function Declarations */ +extern void VelocityEKF(real32_T X[7], real32_T P_prev[49], real32_T + EncoderDiffMeas[3], real32_T eta_encoder, real32_T Accelerometer[3], real32_T + cov_acc[9], real32_T eta_accelerometer, real32_T eta_bias, real32_T qQEKF[4], + real32_T cov_qQEKF[16], real32_T qdotQEKF[4], real32_T eta_acceleration, + real32_T SamplePeriod, real32_T TicksPrRev, real32_T rk, real32_T rw, real32_T + g, real32_T X_out[7], real32_T P_out[49]); +extern void VelocityEKF_api(const mxArray * const prhs[17], int32_T nlhs, const + mxArray *plhs[2]); +extern void VelocityEKF_atexit(void); +extern void VelocityEKF_initialize(void); +extern void VelocityEKF_terminate(void); +extern void VelocityEKF_xil_terminate(void); + +#endif + +/* + * File trailer for _coder_VelocityEKF_api.h + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_info.c b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_info.c new file mode 100644 index 0000000..d28d0a5 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_info.c @@ -0,0 +1,233 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_VelocityEKF_info.c + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +/* Include Files */ +#include "_coder_VelocityEKF_info.h" + +/* Function Definitions */ + +/* + * Arguments : void + * Return Type : const mxArray * + */ +const mxArray *emlrtMexFcnResolvedFunctionsInfo() +{ + const mxArray *nameCaptureInfo; + const char * data[82] = { + "789ced7d5d8c2bc9751eb53fdebb301cd3dad8bb6bade4bd2be1c25e3977867fc399857797ff339c19fe0d39e40cf767d8249b6493cdee667793433279a05f04" + "3d04c61a468204300cc180005b86953502c41b580828c80f7e8b1f943c382f821121b2e107bdd8809e1436bb7bee742feb0eefb058cd6e9e035cd4340f595f75", + "dd53e7549d5375caf58564ea0b2e97eb5fcdfe3d37fbf7cdfff9bc4ba15f72a9e4d6cae75c4632f3bfa0957f6c7ad6e945d70b86dfe9fc3fd4ca1acfc9f45056" + "1f5886a3d3fd6e9516670f1cd5a56faaa9f35d86a338b930126897484b3c3ba0eb734e8361e902d3a54ff95b0f47cceca19bb8c5ba795058cadfd1165debe4fb", + "5d97d8929e3497bdfde0bad53f7f8278ff1796ec9f4344ffb84dfc0fe21f45dfd939976851da29b4f82e25edc4f85abf4b73b2b473d26fb2f4bf49850ba7e1c8" + "4e5c92992e25f3b32f16e327899d22cdf235461ecdfe7edcbd69b78068d72f2cd9eee74ccf3abd6cfabe4aef85d4f2b7433a7e0551ffb2fdf682e9d975f33d95", + "5367a8a652e2c2fb05d3f3133c9553e7fb5596c6271747483c23ff83e4e9c55c34b222df14a9ee9b8a0c4b3b9a2c9c79773dfbd48eccf36c951feed05d768765" + "aa3b33f160a9ea0ecdcefed8517aea71f7ae7e5a562ecca54e2fbb1ee87ffec3b7b9ff1d268837a76dc11b22ea5b56ee7e1581e736f1bd492fd5dccf1c2718a6", + "36ce478fe2a2bf55bed58eec1d3877b5c3857826553f8c5f2319e5ed5d6c7afc81e9d975f33d95c348b3771499a1557afcd315f14e917846feea72a0f7d4ccc8" + "bb48ea9fd0f7df3e047dbea9fafcd710786e133f5deb949262a2d648c817be9a2717885ca62287a0cfb7429f4f0a215cf2f62a02cf6de2d7f83a2d3e66668b3c", + "91a3d8c7528d622931deb46cbebeaa9ecf9a9e5da6efe9fc67960fe5dfd7e7bdb5f375bdbb76f4ee22292793ef3efa18f4bcddf5fcee59d333908f0f78b1d992" + "f209aa1949240731e7e87918c78bdb6fd4f7a91b7d8ff2032ddb5f66ff9dcbf43d9dcf485cbf4b8b4c4d563c75d6f96556958f0c12cfc8bfd73ca0c10ce9bac0", + "cfc463c7d05fb7fc761544fb30cac79f975eff5fa0e7d784474acf1ffa3aa37cafc9b0d142b473d68b340b9df338e8f9add5f3283c7c7e9a86127568d955af1f" + "23f18cfcd5f5bada4faa9786a81e02bdbe463c527ef7418f8e0b839a747071ede3f9ac94f7f82e9de477ff0cf1fb65fbf10251bfdbc4c7add71f361851921b0c", + "51f9fb5185583c55d15b4a69d7786a028967e4e3d0ef8a6e2728079ffdf3df815edf54bdbeec7c3d3f12986ea07756f230d5ba3f59ce65531e27f96560fc1ac9" + "286fbb8ef1c7dc65476a2d4ad9c405fe1844a913ccdbc9e0813f064ffdb69db78f699197faddbbf4ef5af6454e3f0ccdcbd047c4e6f18d3ecb2a25f869d07178", + "49a0c4469fdb51fa8abc9f66f217a0efedafefe53d6ff72c563a6c957dfc7820a7ba8994477690be87fd344632fadd8b37fa7c82a86fd97efa0a02cf6de2cf5e" + "fd6a66deae1abcc8f2bc70c50f68b1c1f2d75735e53cc3eafade4ca8f6e8a4e34def89a7d75f313d9bf174fe4a72a24e139ed27f24e567fabd1fbf08fb6cecae", + "ffa593f8301e1b8fb3c743af3fe5dd8de6c29eddb873f4ff4f10bf5fb61fbf81a8df6de2af7b5c3f7cfa17ae5a342b28c7cec8caeba1657e7d8bf67f325c9d1e" + "263919fc85887683bf9f0c1ef8fbf1d40ff6c1d85ebcf29ac4661feef2dfcc5ebd4b0ded1b1f20e12fd2ce5fcd7b8abcbf68fad9ab2fc37ac1eef640eced0a6d", + "46f287bb7bd92097ad5ef84e8a5204ec01d883c5ef6df43791b5070c07f660397bc07016d883bf027b607f7bd0a42e83195f29dd0b56c372344065fcb43f07e7" + "716feadb8ebc39fb21b53cc0a6df5f42e0bb354e9d6e7a45aa6edbf8f00912cfc85f4dbf2bd161ada734054f50bfbfd5fe00fc3f76d7eff172271729f8a5da79", + "3897c80c8e8397917cc041fa1dc6f1e2f61be5ee0d6c79175e43e0b94d7c93df9da3c426c3d55a1dbbc6873fbc034fe73f939c48b345075ddf51778d697bc7cc" + "5bc86e7a8e74be9dc92bef9cfd10f4ff9af048e9ffd1f03a7940251acdc06e2d13e7f3ed443f78eda0f8f014f17bbb8e6b945c2c2b87cf23de479ff7bf66f8f4", + "ad905a3ed24a7c79335f44b4c3ad711a2ccf2b0e17bbceff93483c237ff579c3bca774f70e497df5fdf34730ffb7bbfedfef0533851e1b0b7422ad42ee302ff7" + "62b15ac239fa1fc6f1e2f61be5ee4d6cfb447f0381e736f1cdfb6ea4489f61e52497564f3759a6f7ed757ec4dc6d44f3b0fd25ecffb1bffecf0c7ad170e368b0", + "5fc8f947b57897f326caec11e8ffedd2ff5fc3a6ffbf84c0739bf80bf36e3ed667fdd69d135855ff9fdf81a7f3f1e8ffafabfd764b7608eaffefc0f930fbebff" + "6a6b2f7bc1b1723dcf77e37deec8578d478ec1ffbfb5faff13447dcbf6d76f21f0dc26be49ff5382c08ef2736596e8733599e1b9249765a99a7ec5ce5d71ef65", + "dbf7cb77b44fe737b4565cb528ae3e5b20e8f87fb3227ef50e7c9d8fc73ea0bbd582fd40d55f6cc37e20bbdb0b9e3a6d5729ea305af7edb7a50b2f9d2a0b2307" + "9d1780f1bdf8bd9693477cf181bbf37c2ad91124daaef1813412cfc8c7913f42ef2bb2e7c3420fde853cfdb6d7f7d7a5a494603bc584b721e6d9413b73cc8d73", + "51d0f7a0ef157aec38bf91557103f01bad0b4fa56dc103bf119efa579ddf6dc7bed1af119bef77c53a3360eab69def9338efc50bd28ede4f8f35471941bdfe7b" + "a33f72f47cdfd27c78a4f2fa0f2fcbe57833cc883dffa01b0f142ec3b936e502bdbe3de358a19fbe4fea9c97f69ab0cff30e79d0c5817c7c68f28f9f7ce868bd", + "6e291ea9f9baaf34e0e3b99227cbc629a976268b99d8091d01bdbe29e35840b46f3de779df0aa9e523d8f7e9827d9fb7dbbd843d807d9f6bc4837d9f78eab7bb" + "3da820da8757eedec6a6ffdf44e0b94dfc05fe7b7a2844f9ae40c9cc4c235ba5ffa72be25d22f18c7c7cf74618bacd02f9097dfcd7afc0bac0ee76a05517c4f2", + "c5385e6b0663a7c191af4f8573fbb02ed8323bf03bd8f23f7c1181e736f14d7660f6f6eae776f503ad2ddeb350ffcfba8b78bc27f4c6cfeaa0efedaeef9bc132" + "9dcdb506f55eb21d2d08a5785a28710ecaf700f9dc8cb4d8fff35e482ddfc716bf45e595706b9c1a2fcd4babf43b31b958399e3feb29b2f73a421e8735e291d2", + "eb4261d80e66e4ec5e304315f2ed827838aa710e9ac7c3f835d2baceefbe8ec0739bf88bf761ea5adebee777f377e0e97cbcfb305599b9fd1e15443b31cee7f7" + "fee39f82deb7bbdedf174f252a574b5d96aa9717d54864cfdf683a29ae0b7adf48706e7771fbe0dcaeb13d706ed71e78706e174ffd30be17bfd772f288ef1cd7", + "a6ac1feeeb2782f5c3e25227583f90c183f5039efa211e60a44d890748cae55d2e8807dcbdae94e6977711d4eb100f58231e29bd4e514791f275e78829a6a27d" + "3612e162c16cda41f37d18bf46dacc7880aee5211ef06cf379e9e6c246827e9e10cce7edaff77703477bfbd1eb927737727dcdee0aac3fd61cc37c7eebf43ec4", + "03548278804a100fb0071ec403f0d40fe37bf17bd92b1e806ffdb05df100583f380d0fd60f78eab76b3cc0fd85c5ed5a56de50ffff7a3ce053c3a727a1793129" + "a9a51e1f085d689f87b4cf535a19d73ed7f8aeb0f6ac95ae9cf6fbbcf6b956860a1abfa83d6be5442ba71a7ea8ac3d6be5e403edf9036ce7d67ec5f4ec327d4f", + "e7cfd4ee5577f61f464b572d9a156851fb1cfc5b4f3fdff8b97e236e9f2627bff26f61dd6277fb749caffb4f86e974b52017fd4d767452ce048f61dd72531f85" + "a8df6de2af613c3facf1dd2ecf5dd55a74ad63dec783f2f7ac279e9d09a96516db3a06f21ba1f08c7cc86ff4ac782a6d0b1ee437c2533fccfb16973a19e5ef18", + "9b1df875049edbc437d9812a4b498fd57e50f976b50305249e918fc99fa5f4db8eda6faae010b403ff2f07eb05dbdb8128bd57122ef66be77b47996bcf794e48" + "648fcb6007b6703c2b846f5fd42a7680e15886d302e276f51b5921376abf11979bbd07bffb43b0036bc223650722552a2c45d2feec80ddcf71b2bc5feaf47a0e", + "baafc68ee3d93a3bf09bd8e206af22f0dc26fec2f876bc69d93ae0d315f1b2a66797e97b3a1f5fbe53a5bb88fa81befb08eeafb4bddedf3d6b7a06f2f1012f36" + "5b523e41352389e4c041f1027bf9759f8ce3870d4694e4064354fea6156cf3ffaf20f0dc26bee2ff9abdfe558317599e17aef8012d3658fe5a8d925837ff9fde", + "134fafbf627a36e3e9fc95f651ab62f494fe23690fa6dffbf18b600fec6e0fa493f8301e1b8fb3c743af3fe5dd8de6c29e5d07e53f75ca3ae013443b97953fd4" + "7c588f17efdefe709208a97f1caae5442b5d47dab356ba9256af1b34ff916c5bbf51ee0e3c9d8fd76f245b713fc2cfbff11ed80bbbdb0ba199941a81616ee03d", + "88cf243218ed25ce218ebc79f6a28268275ef93bc5b67ef83202cf6de22fd2ff724ba4a516cfd65df8e2c82f989e9fb447e5d45ad47cab2b2eb92921f18c7c9c" + "7273d36f64f3697c10ff01ec27b2bb1d48d58b0799f47eff30715c4ff7f84cf63c36f6c1ba614bed40ceea7da5733bd097e82b916e287fdbd68f44f6de34556e", + "6ef59b15f72d413c798d78a4ec81184ef9f3e14634538b4b529b2a265b07b16ed439f6c0aee7e4be8568d7b2f2f6c0f4ac93ee370addfef0e6bc9a7e1e4e3bef" + "76734e4d7b369f7b731543b8e6eda8f6ba354e57bb588e549c5962b826ce38f33112cfc85f759fb2de4f8f05b53e8271e6df1bfd91a3fd447f517addbaf5cfaa", + "f6e05711786e137f78592ec79b6146ecf907dd78a07019ceb5299773ec018ce3c5ed37cadd4fdfc7a5675f42e0b9358ef69ac4f60fe1d6ebcebf5775f28f9f7c" + "e868bd6e291ea979beaf34e0e3b99227cbc629a976268b99d88993f263db7d1c0b88f6ade75cf15b21b57c64b5ffc7b273c5663b60affd6770aed8a97870ae18", + "4ffd76b7071544fbf0cadddbd8f4ff9b083cb789bfe0fc003d14a27c57a064a6ca5ae6df99ae8847d6ffffb96eb3625fd0c77ffd0aac0bec6e075a75412c5f8c" + "e3b56630761a1cf9fa5438b70feb822db303bf836d3fe81711786e13df6407666faf7e6e573fd00912cfc8c7a3ff67dd45fe9eb4377e56077d6f777ddf0c96e9", + "6cae35a8f792ed684128c5d3428973d0fe1fc83bb7f8bd9ecd3f9409a9e5f6e69d03ffd0e2f700ff90b578e01fc2533fe49d5b5cea0479e7d66307b6677f31e4" + "9d5b271ee49dc3533f8ce7c5a54edb9e6f08b79f286b7a7699bea7f321dfd0b2782a6d0b1ee41bc2533f8ce3c5ed37c8dd2475a3ef51fea765fbeb974ccf2ed3", + "f7743e2371aacb421e094a6a51bbeaf90c12cfc8bfd7fab0c10ce9bac0cfc463c7d05f64ef69fd732bf7c33b1d8f949e3ff47546f95e9361a38568e7ac176916" + "3ae771d0f35babe75178b8ce7131524339fdd6b2ab5e5febf90f835e57fbc9827b5341afaf118fd439ae418f8e0b839a747071ede3f9ac94f7f82ecb2ee7e875", + "7bc5e32cce17faa30a36fd7e577e1d456f29a555fa7dd5f3de09249e918f43bf2bba9da01c7c0671d9cdd5ebcbced7f32381e9067a67250f53adfb93e55c36e5" + "71925fc6b67a7d4c8bbcd4bf99b7a3fc256bd9af13fa28342fa71f12d3f38d3ecb2a25cce3d1f17b49a0c4469fdb51fa8afc3c7e6269be02a7e391d2f7f29eb7", + "7b162b1db6ca3e7e3c9053dd44ca233b48dfdb353f4f05d12ebcf276424c9fd719aaa994769db71f21f18cfc95f2f8b3b33f76949e22ea9ffb876f73104fdd54" + "3dbeac3fc69bf452cdfdcc7182616ae37cf4282efa5b4ef2c7c0f8359251dede25e85f9fbda3c80ced3a2f3f45e219f9abcb81de53a4f75185befff621e8f34d", + "d5e7cbcecbd3b54e2929266a8d847ce1ab797281c8652ae2a07b18419f1bc9182f2dc03ec815f1b2a66797e97b3adfbe7175d807b94e3cd80789a77ebbfa5f3e" + "41b46b3df76a9d86d452cb93ec4aa9e554cf9b5cd29e3fc036cf7f1ed13eb7c6a147eac5eba4f4bff9de5d6272b3f2fc60d65364fd76e0af59231e29bd1f28fa", + "0efde2c9a098c904cea483743e970ac830bfdf86f13ba3f7b0cdef5f43e0b94d7cd3fc9ea3c426c3d55a9d55f5bb9950edd049c79bde134fafffc33bf074fe33" + "c987d4a244babea3ceeeb539be79aa7fd373c4cfbbbef2ce19dc876277bd3f1a5e270fa844a319d8ad65e27cbe9de807af1d941f07f4be918c7e9da3d05deb8b", + "65fbe9b710786e13dfa4f7e7b97fc25c3d4fc97d9192e9f850a0664fcc98d6be6fd57c7fd5bc4a55249e918fc7df83ee460bf6d3ffe74301d60376b70b836abf" + "c127bce9dd369fa67dad70fa82f7551d6417607c2f7eafe5e4f16d6c7e9f4ddb7769b603abfafd61df256e3c95b6050ff65de2a91ff4fde2f75a4e1e7ffb46df", + "4f10f539352fa6d91ed8ebbc06e4c5742a1ee4c5c4533fd885c5efb59c3cfa6eecc23711f52ddb8f0f11786e13df64172849a245b948b1ccbc17c26253fb9e55" + "f661ba225e198967e4e391a7cf771f79399a3e17bd067f91dded84d7db191fc4129164f374982f8f8faa470767b283fc4553c4ef615c1bc9185f08113c07a078", + "412462f769e1f613a59178463e0e3f91de5764f331841ebc0bfb436dafe7af4b4929c1768a096f43ccb38376e6981be7a2a0e7b75bcf87c13fe402ffd0ed7683" + "7fc85a3cf00fe1a97f55ff106d7a7699bea7f3d7670f1e3292c29f55caba08cbe3f412dbfcff45049e5be3282fd970d9374e4ce2de4dfd1cf0aca72cb8777302", + "f9d8d68847ec9cc030c5263a837a3bb977313cae8b5e3f97df4b3847dfc3385edc7ea3dc7d0de6fb2e98efdf6e37ccf7adc583f93e9efa1d32dfaff27dae2eb9" + "48cff773d8ecc29711786e13ff737621a9ac756831cacefac6856f3d70d73ed55a8b125d2e7c76a188c433f271d985dbdd46382ef0ef7fed65880bd8dd2e24b8", + "d36c245c4a8f99ee30d316eadd8c9c1e39282e007661f17b2d6717ceb1f981ee9a9fcfbaa04b0dadd3fbabae1f499c17d0d68ff39eb260bfcf67af82beb7bdbe" + "177bbb429b91fce1ee5e36c865ab17be93a214017d0ffa5ea122517dcf70a0ef97d3f70c6781beff2bd0f7f6d7f74dea3298f195d2bd60352c470354c64ffb73", + "0eca0f3445fc1ef6fd18c9b8efe714f4fc9278a0e771e3a9b42d78a0e7f1d43f45fc1ef4bc918c7a3e0dfe9b25f1c07f831b4fa56dc103ff0d9efa21ef9b91d6" + "e59f8173592ac1b9ac65f154da163c389785a77ed0e74632cecf4bd8f6dd7c0581e736f167af7e359b815e357891e579e18a1fd06283e5afafe6292a569fbf9b", + "09d51e9d70adef2aa667339ece5f494ed4e5de53fa8fe4becce9f77efc22e87fbbeb7fe9243e8cc7c6e3ecf1d0eb4f7977a3b9b067d741f917ecb02ee70569a7" + "2bd6990153a71f0bc6f65710edc32b773f7d9fdcbc1eee5d847b173747cf92c6837b17f1d40f7a7d71fb517a9d94dc99f6d5b37d896707f4eaf37aabf47d0689", + "67e4e389db68dd45741d38fd09dcc3b5b9fa7ed97bd3193a7fc9e5bafdc809cf8e8ade52d35bbc3e743947df4f11bfdfccf8ebcd387ea8fd951ea68def5341b4" + "17ab1c862eb1f977de40e0b94d7cb3fea704aad6793c6cd2b2a8a459b0cc0ed8eb5cd5d7d57edb51fb8dac5f7f02f6608d78a4ec41973f2c9ccb8572b62a0f82", + "e3e3cee9612179ee728e3d80f1bcb8d4c9287fef60b3036f22f0dc26fe623bd097e82bf54fbbfaf9c9ce2374b979d26f16ecdb79febffd3bc8bfb0a9f660d975" + "792c7ae40f7776bdf4f5a5ff2477d63b3f6d170407e55f007bb0b8d409756fef0451df9aed814837f4a5c178de0756ad0ba62be211b60737fdb6a3f59b05f776", + "c1fa608d78a4ec41a7386ccae1b4eca7da97f1e362b85617c20d07c507a688dfc3b83692510ef7b0edef7c0981e7d638224db1cac12cbbeed73f41e219f9abc7" + "81b59ed20482e0bcff95d760bfbeedf57c4d4e17f6bcd1642f1d3fae0e7cfddec5a9c70bfb7bb6681c2bf4c4ff2320ea5bb6bf7e1181e736f1957d8aba8e57c8", + "2a3d4f32cf6676350111446640c9f4ceadae23b99f33f45fe03e5e07e8fbbd4097954eb993e160bf4d1d1713c940b60b7e9e2d1ccf0abd89cdcff33a02cf6de2" + "9bfc3c0d96a7e4d4ec3356e3dbd50e9c21f18c7c3cebc127dd6681bfff3b6007ec6f0706fb3e7f903e3ec93462077239489d9d461927d98129e2f7e0df31d2ba", + "fc3bcf23f0dc1a8716a4796995beb7d1b93f4122badf73f27fffe06be0d7b1bb7eafb3f27122210907873213f1d6c2b144552ac59ca3df61fc1ac9286f8fb0cd" + "ebbf84c0739bf89fcb979f50a6a87ab67cfbceeb0b483c231f57befc27dd46fa5cd7e44fbf0cfe7cdbebfdeb868fe98ee2dedd23f1b493f784fd87f96626027a", + "7f3bf4fe2eb1f8ace2b452e6f0dba0d731faf954a17091d443e0b75f271e29bdce95dbfb27e271e7f0f0d427f2f498dfe7b30107cde7613c2f2e755ad73d89e0" + "b747e119f9e0b77f563c95b6050ffcf678ea9f227e0f7e7b2319e530886dde7fd77de8359ee5951d3ba4f6e39bf36eda61fda7e4f598f713d1f55fe80ffffeab", + "e0bfb1bb7e8f8e9b6c92f29d5c148e8571dbb3cb30d47139e11cfd6ed7f12b20dab5acbc3d8768f7cba6efabe40bcd8b4914db3c1fee4347e119f9701ffab3e2" + "a9b42d78701f3a9efaed6a072a8876e195b70398cf2f89e75c3980f9fc3af1603e8fa77e18bf4632e6cf8f6decfc7d55f97ff58ef6e87c737bb83a3d4c7232cc", + "eb4ded8679bdb57830afc753bf5ded0151ffcee42c342fa767c4e6f90d96e795c0a955f3fc4f57c44b22f18cfcd5f66d29b765cd7b4a0fe390d447df3f7f04fa" + "ddeefa7dbf17cc147a6c2cd089b40ab9c3bcdc8bc56a0e9aef6fec7e8bc5fafde1fcda2391e29ab4f13d2a88766295bfec009b7ebf4bdfc23de6708fb9114fa5", + "6dc1837bccf1d40ffa7d71a99341fe2a64f53bdc5f0ef7976f8abe258d07f797e3a9ff6f10bf5fb61feb88fadd263e2efdaedd674a3769f16afed9559d91044a" + "9ee97d837eaa20da8d571e3f82f9fc9278309fc78da7d2b6e0c17c1e4ffda0ef17bfd726ea7b98dfc3fc7e53f42f693c98dfe3a91ff4fde2f75a4a1e274ff4bd", + "45f7e13212753e7bc0372e70edd359d53e90bd2757eb46b2f7e4061efcee0fc13eac098fd478a43bf1da999c0f74bad951ecc2b71b8f84cfdb51b00f56d907a3" + "6d6069ae29b7ae389ee3e866fdf67b5510edc62a8fdfe2b0ede7bce77db97d4e629a1c5dbf49ac66d57a61d538d13912cfc8c7630f0cdd365f3a10dcc7ff6790", + "776173edc2b2f7e5665372f9b0d1ce9deca513adfd83f62873514db8c02e6cba5d1010ed5ecbbece29179a973f7a62272a88fac16fa412f88d70e3a9b42d78e0" + "37c253bf53f57f05d16ebcf2c883df08fc460bdb0f7e236bf1c06f84a77eb00f8bdf6b2979fc5b019b7d780d81e736f117e9e52cdb97ac3affb5aabf2887c433", + "f231d901bdbbac380756fca20cf6c0eef6e0287f747191f7654af22195e9b49327a966e0da41fb8aa688df2fdb8f1544fd6e131f679c59f5011bc3cd64e30755" + "6cf183af20f0dc26be165fbf6af022cbf3c2153fa0c506cb5f5fcd0f4eacee373213aa3d3ad9497e34f3f094fe2399ff61fabd1fbf087e24bbdb05e9243e8cc7", + "c6e3ecf1d0eb4f7977a3b9b067d741f7ad4f11bf87fccd4632cae1bed57165b51366cf0dfe4a56f856d98555d709c53bf0743e26f9d18447ed374576c8c9cde4" + "851fbc0371e54db507cbc69533c1b3f2612d27fb38df305a4fb4e824db8fb8c01e6caa3d40c9c5b272f83ce27df478f26b864fdf0da965582b23e0475a110ffc", + "486bc39bd3b6e0811f094ffd53c4efed6a1f04447bd7734fc07b21b54c825fc9057ea545ef037e256bf1c0af84a7fe29e2f776b51364d711ef87d432a295f8ec" + "c53def8f9c4f8c530cd79734be5dd71364ef8f7cd26d16f829613db1463c5276a2dd0d44e29151ae4c7707aceca7d309df41df41fb58a788dfdbd54e905d4f84", + "426a99da58fbb029fb5ac16e2c2e7502bb41060fec069efaa788dfdbd56e5410edc59b2f23b41176a2c07469b013d8ecc4bc3bc9cb5308ecc41af148d989ee65" + "edd4b797cd1f9e0ac7c17a2cd51bf0a59683ee3f98227e6f573bf12d447b9795c307a6679df4f545e8f6879398fa38d1d6199388c64e6a655cfb3ca595dab32b", + "bd71716fa7d8178887af0d6f4edb8207f1703cf54f11bfb7ab7da920da8b570e23d8d621bf8ec0739bf826bd5c6529e93133a4e6b938142215cf9018ae89339e" + "5140e219f998e447e9b71db5df54c921b8def8f937de83b8b7dded41b5c49f9f662f1ad5eee9385f8e54a34de9bce9a0f5068ce7c5a54e46f9f36e861de05886", + "5bf23ea24d3d476189dcccfb8db8dcec41fe0dfbdb8148950a4b91b43f3b60f7739c2cef973abd9e83d605b61ccf96d9013f363bf01b083cb789bfc80ef425fa" + "6ab64052feb6ed7ed8cb3bf0743e4eb9b9d56f56f889c01eac118f58bebe70ca9f0f37a2995a5c92da5431d93a88751d948f09ecc1e25227a3fc05b1d9832f23", + "f0dc26bec91ee8eaecc6456457ff50098967e4e3f32f1a4587a07fe83b90b7dbfe7680ab5e97c2345b0df60bc1047f9e089c8e4e03b02ed8b8f12c20dab99e7d" + "aec1905abeb329766158a3aa92c70576e119e548ed37b00b8ec12365170ae74785ced043b703be28d7da3de8c6cf5a4d581f6ce17856e8710897de459dcb736b", + "9cd9ebcd4babf4fc9fac887788c433f257bbafa1d1e776663d45360ff7f4fc11e875bbebf55eb55a6ff8856aa690cd8b39de7378c89efa1d744e01c6af918cf2" + "f6a6d57e7f468af419564e72e97e9716999a6de7f317483c231fd7bd0bc66e2399ff62f297fffc77a0f7edaef733835e34dc381aec1772fe512ddee5bc89327b", + "047a7f3bf4feaed5e7d0a41ac552e2637d766fdf7d3ff93bf0743ea6759fda6f9accdc7e8f0aa29d18e7fbbfff7f207faaedf57e3b70986bd673c5e44122ed39" + "285673816623eb203f0ee87d23a1f4fe3711f52ddb4f6f21f0dc26be49ef5382c08ef2732596e8733599e139f57b77c535966dd72fdfd12e9ddfd0d0af5a1457", + "9f2d0470edfff9e00e7c9d8fc71e2ce84ef276e1efffa90ae702ec6e174e8a9e0eef1f37a8faf575e9cceb4f1d24ce2f211fde168f6b8502d8ecc55711786e13" + "df6c2f063c530f8b22354ab0942cd31cc335e7dfb3ebfed00fefc0d3f998e46841f711df4f36f917b00fb6b70fe3e3235ff45cb8e8e6aa997af8a271941a7633", + "0ef2174d11bf07fb6024a31c1e39ce8f6455fc00fc48ebc253695bf0c08f84a77e9bee07b260dfb742f8ee6383fb149e8ea7f3e13e85fbe2a9b42d78709f029e" + "fac11e2c2e7532cadf410897fc41fe39232ee49f5b1bde9cb6050ff2cfe1a97f8af83de49f3392510e1356fb8dd44364d23525687cf01b2d7f2e7dde6fe4fd46", + "907f6e8d78a4ee6f1e84d9339f54cbed49fc882df6ab0329ef29b99c630f603c2f2e7532ca1fbef3c4907feee978907feede7873da163cc83f87a77eb0038b4b" + "9d8cf2f79ed5e7d020ff1ce49f5b166f4edb8207f9e7f0d40ff66071a99351fedec7660fde40e0b94d7c549e21dd456457ff50118967e4e3ce4ba28b0e41ffd0", + "57ff4307f615d9dd0e548548eb3499c9848fb38dc333af3f209ef217b02ed8b8f1acfbcdef2b7fcf21de6371fe396f482d7d215c7af859f30e6d4afc18ce371a" + "09f21191c1837c4478ea87f16b24bbe423da14fd0f798a16973a419e22327890a7084ffd53c4efe1dc9991ec72eecc297602cea3ad0b4fa56dc183f36878ea77", + "8adfa88268e7a6de6706e7d19e8ea7f3e13cda7df154da163c388f86a77eb0078b4b9d8cf2b767b55f49dd672aa5fb2c1bef0af288583cb9cef7abb7e3c9d315" + "f1acd85f74abdfacd85ff43f7e1fe2ca1b6b0f963e7790a2c619c19fa3e5d4f0ac1b142fa862b4e0728e3d98227e0fe3da4846397c842d9efc1202cfad711889", + "eef529d632bdffe98a7827483c237fb5b8d3ec8f1dada788fb81269f41bc6073f5fcb2f3fefc79af2fa793c5e2f9593eb727f6c247019a7250fc18c6f1e2f6a3" + "e2c802a2be65fb0bf5ffe3369573ff85fac657355ea4ed3abfff088967e42bf2915d4d400491195032bd63eebab9c490939750f0072cf87becaef70fea612a16", + "640eae738562aded392a5f570f7b0ef2f74c11bf87716d24a31cfea6d57988384a6c325cadd5b1abff7f2df9aaa51625d2f51d7555a8ad0dcd4bc49b9e237dbe" + "78f2ca3b67709ecceef66034bc4e1e50894633b05bcbc4f97c3bd10f5e3bc81efc2de2f7cbf6238ba8df6de2afd51e3cac32b3613ed23fbdfd7e1544fbb1cae5", + "f41c9bffe781e9d975f33d95c3489240899265eb8255d78d69249e91bfcaba51e92165e7b1de5764e344a107ef7e0ceb00bbebfdeb52524ab09d62c2db10f3ec" + "a09d39e6c639d8077453dfa6c47d517e99f59c1f0b85e6c52462f57a00f292425ed267c59bd3b6e0415e523cf54f11bf87bca44632c8e12489cd3e7c1181e736", + "f14d7ab9ce0cd4cf49ad13ccf926565d27ac2dbeb4506e66dd453cbe147ae367755827d85dff3783653a9b6b0deabd643b5a104af1b450e21ce41f9a227e0ffa" + "df4846394c597dae4c4d3ad4a4c5bec6b7ca0ed8ecfc98bac854fa8dfcbea19f7c9b037b60777bd03ae0ba194f7db4d7ad3252fb548c5c7aa418f88db6703c2b", + "14c36607202ff5d3f1202ff5bdf1e6b42d7890971a4ffd4eb10364e307f1d0bc98c4e11c990bce9b2c7a9f25f4199c235b231e9c23c353bf53ec4305d14ebcf1" + "8227f79859144fbef117d9d54f7486c433f271cb0b79ff22f889d689476a7d30b808b6f6f28d5caae1a11aa9f220166c1f041d143780f1bcb8d4c9287f11f013", + "b9c04f74bbdde027b2160ffc4478ea778a1d20eb278a86e6c5240a7e2217f88916bd0ff889acc5033f119efa9d621f2a8876e2f513e18b27c3fd964fc783fb2d" + "57c69bd3b6e0c1fd9678ea077bb0b8d40915379820ea5bb6dfbe84c0739bf8a8fb2d675d30e7db357e708ec433f2719f67d44587e0b983ef945e877581dded80", + "3f7692f29d15fdde662b9f1c4b99ddfc482a44c00e6cdf7856e831363bb0ea3dc7b3f79f0f01bbda01abceb52bfd06f71c3b078f941d181e66f7bd87c3b3cee9" + "4139e72d642e87f94006ecc0c68d67a2f1834924a4fef1247e5041d40ff71c3fbd1dcebd2715ee395e271edc738ca77ea7e8ff0aa29d78fd41f8e2c5f73c773c", + "d7bf2986eb9befb3acacd89e678d17dbcb8ff8a4db2cf023421ea235e211bbb7b21b88c423a35c99ee0e58d94fa713be833ed8818db30364d701b190fac70936" + "bb00f7583e1d4fe7c33d96f7c553695bf0e01e4b3cf5837d50e9d9f699eae791f1d907d8678ac233f2619fe9bdf1e6b42d78b0cf144ffd4eb10f15443bf1cadf", + "137bb029f9ad2b2bb683b4ff08f258af0d6f4edb820779acf1d4ef14fd3f41b47359f97bc1f4ac93be3e78f3f687935c48fde3442d2719edf96ce3ecc3a6c49b" + "c16e2c2e7502bb41060fec069efaa788dfafe3beccd5c7f36c24f3ec40b911ed9a12b92b6573649fa544176939cc63b30fcbcaa1492f2baf3f7b797cf16794dd", + "746b9c5a6bdecdd8e4662d79d397b92f53eb390be2d0ff15ce25d8df1e04c421ebf349e1fa71b54f651ab1eb6c71cc3a280e3d45fc7e33ef4378620fb4bfd2c3" + "b4f17d2a88f66295c350199b3db8e73c5daa51333b186f5a762ee1d315f1b2a66797e97b3a1f8fdce8dd4532ce3cf9ee23b837d3f6fa7ff7ace919c8c707bcd8", + "6c49f904d58c24928318e8ff4dd5ff02a2bd6b8937873e0ccdcbe98756ef53550fabc9a2b4acdf6353cfab59922757e937e27972a73fffc67b601fec6e1fc2de" + "4e90e9d76abe7e35727d5df378bc114f12f2576ce17856e8189b1d80bca74fc783bca7f7c69bd3b6e041de533cf5831d585cea6494bfd38dd87f0a79ed20afdd", + "a6eb67d27890d70e4ffd600f16973a19e52f03f18215f1205e801b4fa56dc18378019efa41ef2f2e7532e6af48615b07ac9ac74e0f0dd8352e60d57e655d7408" + "da817ff9a72cec1bb2bb1db848c7e2bb29ca77745ef0ef774f72b554e77417fc415b389e15f2c3fc7f453c98ffe3c653695bf060fe8fa77ea7e8fdff8468e7b2", + "f2f712e23df47d42fbb73ffc513da49635ada4b4b2a296215a2bab6af9660df21bb9f0c40f2aa667339ece87fc46f7c553695bf020bf119efa9d62472a8876e2" + "95bf1ab6f5c3bf46e0b94dfccff9916a8ca07c6ed7f5430a8967e4e39197797791be37e3bfff14fc461babf797cd5b24c4d3b1d3e268f760dccb9fd47ac378e0", + "2cdd743947ef4f11bf87f306465a7cdee0a390fac7c7b03e70c1fa60d1fbc0fac05a3c581fac56ffff07f766c6c7", + "" }; + + nameCaptureInfo = NULL; + emlrtNameCaptureMxArrayR2016a(data, 249760U, &nameCaptureInfo); + return nameCaptureInfo; +} + +/* + * Arguments : void + * Return Type : mxArray * + */ +mxArray *emlrtMexFcnProperties() +{ + mxArray *xResult; + mxArray *xEntryPoints; + const char * fldNames[6] = { "Name", "NumberOfInputs", "NumberOfOutputs", + "ConstantInputs", "FullPath", "TimeStamp" }; + + mxArray *xInputs; + const char * b_fldNames[4] = { "Version", "ResolvedFunctions", "EntryPoints", + "CoverageInfo" }; + + xEntryPoints = emlrtCreateStructMatrix(1, 1, 6, *(const char * (*)[6])& + fldNames[0]); + xInputs = emlrtCreateLogicalMatrix(1, 17); + emlrtSetField(xEntryPoints, 0, "Name", emlrtMxCreateString("VelocityEKF")); + emlrtSetField(xEntryPoints, 0, "NumberOfInputs", emlrtMxCreateDoubleScalar + (17.0)); + emlrtSetField(xEntryPoints, 0, "NumberOfOutputs", emlrtMxCreateDoubleScalar + (2.0)); + emlrtSetField(xEntryPoints, 0, "ConstantInputs", xInputs); + emlrtSetField(xEntryPoints, 0, "FullPath", emlrtMxCreateString( + "C:\\Users\\Thomas\\Documents\\Kugle-MATLAB\\Estimators\\VEKF\\VelocityEKF.m")); + emlrtSetField(xEntryPoints, 0, "TimeStamp", emlrtMxCreateDoubleScalar + (737541.56930555555)); + xResult = emlrtCreateStructMatrix(1, 1, 4, *(const char * (*)[4])&b_fldNames[0]); + emlrtSetField(xResult, 0, "Version", emlrtMxCreateString( + "9.4.0.813654 (R2018a)")); + emlrtSetField(xResult, 0, "ResolvedFunctions", (mxArray *) + emlrtMexFcnResolvedFunctionsInfo()); + emlrtSetField(xResult, 0, "EntryPoints", xEntryPoints); + return xResult; +} + +/* + * File trailer for _coder_VelocityEKF_info.c + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_info.h b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_info.h new file mode 100644 index 0000000..960de35 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_info.h @@ -0,0 +1,28 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_VelocityEKF_info.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +#ifndef _CODER_VELOCITYEKF_INFO_H +#define _CODER_VELOCITYEKF_INFO_H +/* Include Files */ +#include "tmwtypes.h" +#include "mex.h" +#include "emlrt.h" + + +/* Function Declarations */ +extern const mxArray *emlrtMexFcnResolvedFunctionsInfo(); +MEXFUNCTION_LINKAGE mxArray *emlrtMexFcnProperties(); + +#endif +/* + * File trailer for _coder_VelocityEKF_info.h + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_mex.cpp b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_mex.cpp new file mode 100644 index 0000000..14d36be --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_mex.cpp @@ -0,0 +1,101 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_VelocityEKF_mex.cpp + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +/* Include Files */ +#include "_coder_VelocityEKF_api.h" +#include "_coder_VelocityEKF_mex.h" + +/* Function Declarations */ +static void VelocityEKF_mexFunction(int32_T nlhs, mxArray *plhs[2], int32_T nrhs, + const mxArray *prhs[17]); + +/* Function Definitions */ + +/* + * Arguments : int32_T nlhs + * mxArray *plhs[2] + * int32_T nrhs + * const mxArray *prhs[17] + * Return Type : void + */ +static void VelocityEKF_mexFunction(int32_T nlhs, mxArray *plhs[2], int32_T nrhs, + const mxArray *prhs[17]) +{ + const mxArray *outputs[2]; + int32_T b_nlhs; + emlrtStack st = { NULL, /* site */ + NULL, /* tls */ + NULL /* prev */ + }; + + st.tls = emlrtRootTLSGlobal; + + /* Check for proper number of arguments. */ + if (nrhs != 17) { + emlrtErrMsgIdAndTxt(&st, "EMLRT:runTime:WrongNumberOfInputs", 5, 12, 17, 4, + 11, "VelocityEKF"); + } + + if (nlhs > 2) { + emlrtErrMsgIdAndTxt(&st, "EMLRT:runTime:TooManyOutputArguments", 3, 4, 11, + "VelocityEKF"); + } + + /* Call the function. */ + VelocityEKF_api(prhs, nlhs, outputs); + + /* Copy over outputs to the caller. */ + if (nlhs < 1) { + b_nlhs = 1; + } else { + b_nlhs = nlhs; + } + + emlrtReturnArrays(b_nlhs, plhs, outputs); + + /* Module termination. */ + VelocityEKF_terminate(); +} + +/* + * Arguments : int32_T nlhs + * mxArray * const plhs[] + * int32_T nrhs + * const mxArray * const prhs[] + * Return Type : void + */ +void mexFunction(int32_T nlhs, mxArray *plhs[], int32_T nrhs, const mxArray + *prhs[]) +{ + mexAtExit(VelocityEKF_atexit); + + /* Initialize the memory manager. */ + /* Module initialization. */ + VelocityEKF_initialize(); + + /* Dispatch the entry-point. */ + VelocityEKF_mexFunction(nlhs, plhs, nrhs, prhs); +} + +/* + * Arguments : void + * Return Type : emlrtCTX + */ +emlrtCTX mexFunctionCreateRootTLS(void) +{ + emlrtCreateRootTLS(&emlrtRootTLSGlobal, &emlrtContextGlobal, NULL, 1); + return emlrtRootTLSGlobal; +} + +/* + * File trailer for _coder_VelocityEKF_mex.cpp + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_mex.h b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_mex.h new file mode 100644 index 0000000..973770f --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/interface/_coder_VelocityEKF_mex.h @@ -0,0 +1,34 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: _coder_VelocityEKF_mex.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +#ifndef _CODER_VELOCITYEKF_MEX_H +#define _CODER_VELOCITYEKF_MEX_H + +/* Include Files */ +#include +#include +#include +#include "tmwtypes.h" +#include "mex.h" +#include "emlrt.h" +#include "_coder_VelocityEKF_api.h" + +/* Function Declarations */ +extern void mexFunction(int32_T nlhs, mxArray *plhs[], int32_T nrhs, const + mxArray *prhs[]); +extern emlrtCTX mexFunctionCreateRootTLS(void); + +#endif + +/* + * File trailer for _coder_VelocityEKF_mex.h + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/mrdivide.cpp b/Estimators/VEKF/codegen/lib/VelocityEKF/mrdivide.cpp new file mode 100644 index 0000000..d3215b2 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/mrdivide.cpp @@ -0,0 +1,157 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: mrdivide.cpp +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// + +// Include Files +#include +#include "rt_nonfinite.h" +#include "VelocityEKF.h" +#include "mrdivide.h" + +// Function Definitions + +// +// Arguments : const float A[42] +// const float B[36] +// float y[42] +// Return Type : void +// +void mrdivide(const float A[42], const float B[36], float y[42]) +{ + int k; + int j; + int jAcol; + int c; + float b_A[36]; + int jy; + int ix; + signed char ipiv[6]; + float b_B[42]; + float smax; + int iy; + int i; + float s; + for (k = 0; k < 6; k++) { + for (jAcol = 0; jAcol < 6; jAcol++) { + b_A[jAcol + 6 * k] = B[k + 6 * jAcol]; + } + + for (jAcol = 0; jAcol < 7; jAcol++) { + b_B[jAcol + 7 * k] = A[k + 6 * jAcol]; + } + + ipiv[k] = (signed char)(1 + k); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + jAcol = 0; + ix = c; + smax = (float)fabs((double)b_A[c]); + for (k = 2; k <= 6 - j; k++) { + ix++; + s = (float)fabs((double)b_A[ix]); + if (s > smax) { + jAcol = k - 1; + smax = s; + } + } + + if (b_A[c + jAcol] != 0.0F) { + if (jAcol != 0) { + ipiv[j] = (signed char)((j + jAcol) + 1); + ix = j; + iy = j + jAcol; + for (k = 0; k < 6; k++) { + smax = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = smax; + ix += 6; + iy += 6; + } + } + + k = (c - j) + 6; + for (i = c + 1; i < k; i++) { + b_A[i] /= b_A[c]; + } + } + + iy = c; + jy = c + 6; + for (jAcol = 1; jAcol <= 5 - j; jAcol++) { + smax = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + k = (iy - j) + 12; + for (i = 7 + iy; i < k; i++) { + b_A[i] += b_A[ix] * -smax; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + for (j = 0; j < 6; j++) { + jy = 7 * j; + jAcol = 6 * j; + for (k = 1; k <= j; k++) { + iy = 7 * (k - 1); + if (b_A[(k + jAcol) - 1] != 0.0F) { + for (i = 0; i < 7; i++) { + b_B[i + jy] -= b_A[(k + jAcol) - 1] * b_B[i + iy]; + } + } + } + + smax = 1.0F / b_A[j + jAcol]; + for (i = 0; i < 7; i++) { + b_B[i + jy] *= smax; + } + } + + for (j = 5; j >= 0; j--) { + jy = 7 * j; + jAcol = 6 * j - 1; + for (k = j + 2; k < 7; k++) { + iy = 7 * (k - 1); + if (b_A[k + jAcol] != 0.0F) { + for (i = 0; i < 7; i++) { + b_B[i + jy] -= b_A[k + jAcol] * b_B[i + iy]; + } + } + } + } + + for (jAcol = 4; jAcol >= 0; jAcol--) { + if (ipiv[jAcol] != jAcol + 1) { + iy = ipiv[jAcol] - 1; + for (jy = 0; jy < 7; jy++) { + smax = b_B[jy + 7 * jAcol]; + b_B[jy + 7 * jAcol] = b_B[jy + 7 * iy]; + b_B[jy + 7 * iy] = smax; + } + } + } + + for (k = 0; k < 7; k++) { + for (jAcol = 0; jAcol < 6; jAcol++) { + y[jAcol + 6 * k] = b_B[k + 7 * jAcol]; + } + } +} + +// +// File trailer for mrdivide.cpp +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/mrdivide.h b/Estimators/VEKF/codegen/lib/VelocityEKF/mrdivide.h new file mode 100644 index 0000000..06f6a3f --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/mrdivide.h @@ -0,0 +1,28 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// File: mrdivide.h +// +// MATLAB Coder version : 4.0 +// C/C++ source code generated on : 26-Apr-2019 13:40:20 +// +#ifndef MRDIVIDE_H +#define MRDIVIDE_H + +// Include Files +#include +#include +#include "rtwtypes.h" +#include "VelocityEKF_types.h" + +// Function Declarations +extern void mrdivide(const float A[42], const float B[36], float y[42]); + +#endif + +// +// File trailer for mrdivide.h +// +// [EOF] +// diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetInf.cpp b/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetInf.cpp new file mode 100644 index 0000000..8973485 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetInf.cpp @@ -0,0 +1,129 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rtGetInf.cpp + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, Inf and MinusInf + */ +#include "rtGetInf.h" + +/* Function: rtGetInf ================================================== + * Abstract: + * Initialize rtInf needed by the generated code. + */ +real_T rtGetInf(void) +{ + real_T inf = 0.0; + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + } + + return inf; +} + +/* Function: rtGetInfF ================================================== + * Abstract: + * Initialize rtInfF needed by the generated code. + */ +real32_T rtGetInfF(void) +{ + IEEESingle infF; + infF.wordL.wordLuint = 0x7F800000U; + return infF.wordL.wordLreal; +} + +/* Function: rtGetMinusInf ================================================== + * Abstract: + * Initialize rtMinusInf needed by the generated code. + */ +real_T rtGetMinusInf(void) +{ + real_T minf = 0.0; + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + } + + return minf; +} + +/* Function: rtGetMinusInfF ================================================== + * Abstract: + * Initialize rtMinusInfF needed by the generated code. + */ +real32_T rtGetMinusInfF(void) +{ + IEEESingle minfF; + minfF.wordL.wordLuint = 0xFF800000U; + return minfF.wordL.wordLreal; +} + +/* + * File trailer for rtGetInf.cpp + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetInf.h b/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetInf.h new file mode 100644 index 0000000..de49faa --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetInf.h @@ -0,0 +1,28 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rtGetInf.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +#ifndef RTGETINF_H +#define RTGETINF_H +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetInf(void); +extern real32_T rtGetInfF(void); +extern real_T rtGetMinusInf(void); +extern real32_T rtGetMinusInfF(void); + +#endif + +/* + * File trailer for rtGetInf.h + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetNaN.cpp b/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetNaN.cpp new file mode 100644 index 0000000..6ccb716 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetNaN.cpp @@ -0,0 +1,96 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rtGetNaN.cpp + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, NaN + */ +#include "rtGetNaN.h" + +/* Function: rtGetNaN ================================================== + * Abstract: + * Initialize rtNaN needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetNaN(void) +{ + real_T nan = 0.0; + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF80000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + nan = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; + tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; + nan = tmpVal.fltVal; + break; + } + } + + return nan; +} + +/* Function: rtGetNaNF ================================================== + * Abstract: + * Initialize rtNaNF needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetNaNF(void) +{ + IEEESingle nanF = { { 0 } }; + + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + nanF.wordL.wordLuint = 0xFFC00000U; + break; + } + + case BigEndian: + { + nanF.wordL.wordLuint = 0x7FFFFFFFU; + break; + } + } + + return nanF.wordL.wordLreal; +} + +/* + * File trailer for rtGetNaN.cpp + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetNaN.h b/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetNaN.h new file mode 100644 index 0000000..ee5de49 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/rtGetNaN.h @@ -0,0 +1,26 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rtGetNaN.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +#ifndef RTGETNAN_H +#define RTGETNAN_H +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetNaN(void); +extern real32_T rtGetNaNF(void); + +#endif + +/* + * File trailer for rtGetNaN.h + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/rt_nonfinite.cpp b/Estimators/VEKF/codegen/lib/VelocityEKF/rt_nonfinite.cpp new file mode 100644 index 0000000..e36a498 --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/rt_nonfinite.cpp @@ -0,0 +1,83 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rt_nonfinite.cpp + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finites, + * (Inf, NaN and -Inf). + */ +#include "rt_nonfinite.h" +#include "rtGetNaN.h" +#include "rtGetInf.h" + +real_T rtInf; +real_T rtMinusInf; +real_T rtNaN; +real32_T rtInfF; +real32_T rtMinusInfF; +real32_T rtNaNF; + +/* Function: rt_InitInfAndNaN ================================================== + * Abstract: + * Initialize the rtInf, rtMinusInf, and rtNaN needed by the + * generated code. NaN is initialized as non-signaling. Assumes IEEE. + */ +void rt_InitInfAndNaN(size_t realSize) +{ + (void)realSize; + rtNaN = rtGetNaN(); + rtNaNF = rtGetNaNF(); + rtInf = rtGetInf(); + rtInfF = rtGetInfF(); + rtMinusInf = rtGetMinusInf(); + rtMinusInfF = rtGetMinusInfF(); +} + +/* Function: rtIsInf ================================================== + * Abstract: + * Test if value is infinite + */ +boolean_T rtIsInf(real_T value) +{ + return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); +} + +/* Function: rtIsInfF ================================================= + * Abstract: + * Test if single-precision value is infinite + */ +boolean_T rtIsInfF(real32_T value) +{ + return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); +} + +/* Function: rtIsNaN ================================================== + * Abstract: + * Test if value is not a number + */ +boolean_T rtIsNaN(real_T value) +{ + return (value!=value)? 1U:0U; +} + +/* Function: rtIsNaNF ================================================= + * Abstract: + * Test if single-precision value is not a number + */ +boolean_T rtIsNaNF(real32_T value) +{ + return (value!=value)? 1U:0U; +} + +/* + * File trailer for rt_nonfinite.cpp + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/rt_nonfinite.h b/Estimators/VEKF/codegen/lib/VelocityEKF/rt_nonfinite.h new file mode 100644 index 0000000..36a515e --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/rt_nonfinite.h @@ -0,0 +1,58 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rt_nonfinite.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +#ifndef RT_NONFINITE_H +#define RT_NONFINITE_H +#if defined(_MSC_VER) && (_MSC_VER <= 1200) +#include +#endif + +#include +#include "rtwtypes.h" + +extern real_T rtInf; +extern real_T rtMinusInf; +extern real_T rtNaN; +extern real32_T rtInfF; +extern real32_T rtMinusInfF; +extern real32_T rtNaNF; +extern void rt_InitInfAndNaN(size_t realSize); +extern boolean_T rtIsInf(real_T value); +extern boolean_T rtIsInfF(real32_T value); +extern boolean_T rtIsNaN(real_T value); +extern boolean_T rtIsNaNF(real32_T value); +typedef struct { + struct { + uint32_T wordH; + uint32_T wordL; + } words; +} BigEndianIEEEDouble; + +typedef struct { + struct { + uint32_T wordL; + uint32_T wordH; + } words; +} LittleEndianIEEEDouble; + +typedef struct { + union { + real32_T wordLreal; + uint32_T wordLuint; + } wordL; +} IEEESingle; + +#endif + +/* + * File trailer for rt_nonfinite.h + * + * [EOF] + */ diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/rtw_proj.tmw b/Estimators/VEKF/codegen/lib/VelocityEKF/rtw_proj.tmw new file mode 100644 index 0000000..1707d8f --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/rtw_proj.tmw @@ -0,0 +1 @@ +Code generation project for VelocityEKF using toolchain "GNU Tools for ARM Embedded Processors v5.2 | gmake (64-bit Windows)". MATLAB root = C:\Program Files\MATLAB\R2018a. diff --git a/Estimators/VEKF/codegen/lib/VelocityEKF/rtwtypes.h b/Estimators/VEKF/codegen/lib/VelocityEKF/rtwtypes.h new file mode 100644 index 0000000..fdf446c --- /dev/null +++ b/Estimators/VEKF/codegen/lib/VelocityEKF/rtwtypes.h @@ -0,0 +1,144 @@ +/* + * Academic License - for use in teaching, academic research, and meeting + * course requirements at degree granting institutions only. Not for + * government, commercial, or other organizational use. + * File: rtwtypes.h + * + * MATLAB Coder version : 4.0 + * C/C++ source code generated on : 26-Apr-2019 13:40:20 + */ + +#ifndef RTWTYPES_H +#define RTWTYPES_H + +/*=======================================================================* + * Target hardware information + * Device type: ARM Compatible->ARM Cortex + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 + * native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * + * ulong_T, char_T and byte_T. * + *===========================================================================*/ +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef char_T byte_T; + +/*===========================================================================* + * Complex number type definitions * + *===========================================================================*/ +#define CREAL_T + +typedef struct { + real32_T re; + real32_T im; +} creal32_T; + +typedef struct { + real64_T re; + real64_T im; +} creal64_T; + +typedef struct { + real_T re; + real_T im; +} creal_T; + +typedef struct { + int8_T re; + int8_T im; +} cint8_T; + +typedef struct { + uint8_T re; + uint8_T im; +} cuint8_T; + +typedef struct { + int16_T re; + int16_T im; +} cint16_T; + +typedef struct { + uint16_T re; + uint16_T im; +} cuint16_T; + +typedef struct { + int32_T re; + int32_T im; +} cint32_T; + +typedef struct { + uint32_T re; + uint32_T im; +} cuint32_T; + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255)) +#define MIN_uint8_T ((uint8_T)(0)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535)) +#define MIN_uint16_T ((uint16_T)(0)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MIN_uint32_T ((uint32_T)(0)) + +/* Logical type definitions */ +#if !defined(__cplusplus) && !defined(__true_false_are_keywords) && !defined(__bool_true_false_are_defined) +# ifndef false +# define false (0U) +# endif + +# ifndef true +# define true (1U) +# endif +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable) + * including the null-termination character. Referenced by + * rt_logging.c and rt_matrx.c. + */ +#define TMW_NAME_LENGTH_MAX 64 +#endif + +/* + * File trailer for rtwtypes.h + * + * [EOF] + */