Aircraft Position Radar Model
This model shows the code generated for a Simulink® model containing a MATLAB® script.
The model contains an Extended Kalman Filter that estimates aircraft position from radar measurements. The MATLAB script AircraftPositionData.m contains data for running the model. The estimated and actual positions are saved to the workspace and are plotted at the end of the simulation by the program AircraftPositionPlot.m (called from the simulation automatically).
Review and Simulate the Model
Review the model and perform a simulation.
Open the Simulink model.
model='AircraftPositionRadar'; open_system(model) AircraftPositionRadar([],[],[],'compile'); AircraftPositionRadar([],[],[],'term');
Open the MATLAB Function block RadarTracker
in the MATLAB Editor.
open_system([model,'/RadarTracker'])
Simulate the model and review the results.
sim(model)
Generate Code for the Model
Generate code for the Kalman Filter portion of the model using the subsystem build functionality provided by Simulink Coder. In the first build, the model is configured to generate code using Simulink Coder™. In the second build, the model is configured to generate code using Embedded Coder®.
Configure and build the model using Simulink Coder.
set_param(model, "SystemTargetFile", "grt.tlc"); slbuild([model,'/RadarTracker'])
### Starting build procedure for: RadarTracker ### Successful completion of build procedure for: RadarTracker Build Summary Top model targets: Model Build Reason Status Build Duration =============================================================================================================== RadarTracker Information cache folder or artifacts were missing. Code generated and compiled. 0h 0m 12.971s 1 of 1 models built (0 models already up to date) Build duration: 0h 0m 13.384s
Configure and build the model using Embedded Coder.
set_param(model, "SystemTargetFile", "ert.tlc"); slbuild([model,'/RadarTracker'])
### Starting build procedure for: RadarTracker ### Successful completion of build procedure for: RadarTracker Build Summary Top model targets: Model Build Reason Status Build Duration =============================================================================================================== RadarTracker Information cache folder or artifacts were missing. Code generated and compiled. 0h 0m 12.85s 1 of 1 models built (0 models already up to date) Build duration: 0h 0m 13.32s
A portion of RadarTracker.c
is listed below.
cfile = fullfile(pwd,'RadarTracker_ert_rtw','RadarTracker.c'); coder.example.extractLines(cfile,'/* Model step', '/* Model initialize', 1, 0);
/* Model step function */ void RadarTracker_step(void) { __m128d tmp_0; __m128d tmp_1; __m128d tmp_2; real_T P_tmp[16]; real_T Phi_0[16]; real_T Q[16]; real_T Q_0[16]; real_T M[8]; real_T W[8]; real_T tmp[8]; real_T x_tmp[8]; real_T Phi_1[4]; real_T tmp_3[2]; real_T Bearinghat; real_T P; real_T P_0; real_T P_1; real_T Phi_2; real_T Phi_3; real_T Rangehat; real_T r; int32_T P_tmp_0; int32_T i; int32_T j; int32_T tmp_4; int32_T tmp_5; int32_T x_tmp_tmp; int8_T Phi[16]; static const real_T e[4] = { 0.0, 0.005, 0.0, 0.005 }; static const real_T c_b[4] = { 90000.0, 0.0, 0.0, 1.0E-6 }; /* MATLAB Function: '<Root>/RadarTracker' incorporates: * Inport: '<Root>/meas' */ Phi[0] = 1; Phi[4] = 1; Phi[8] = 0; Phi[12] = 0; Phi[2] = 0; Phi[6] = 0; Phi[10] = 1; Phi[14] = 1; Phi[1] = 0; Phi[3] = 0; Phi[5] = 1; Phi[7] = 0; Phi[9] = 0; Phi[11] = 0; Phi[13] = 0; Phi[15] = 1; memset(&Q[0], 0, sizeof(real_T) << 4U); for (j = 0; j < 4; j++) { Q[j + (j << 2)] = e[j]; } for (i = 0; i < 4; i++) { P_tmp_0 = i << 2; r = RadarTracker_DW.P[P_tmp_0 + 1]; P = RadarTracker_DW.P[P_tmp_0]; P_0 = RadarTracker_DW.P[P_tmp_0 + 2]; P_1 = RadarTracker_DW.P[P_tmp_0 + 3]; for (j = 0; j <= 2; j += 2) { _mm_storeu_pd(&Phi_0[j + P_tmp_0], _mm_add_pd(_mm_add_pd(_mm_add_pd (_mm_mul_pd(_mm_set1_pd(r), _mm_set_pd(Phi[j + 5], Phi[j + 4])), _mm_mul_pd(_mm_set1_pd(P), _mm_set_pd(Phi[j + 1], Phi[j]))), _mm_mul_pd (_mm_set1_pd(P_0), _mm_set_pd(Phi[j + 9], Phi[j + 8]))), _mm_mul_pd (_mm_set1_pd(P_1), _mm_set_pd(Phi[j + 13], Phi[j + 12])))); } } for (i = 0; i < 4; i++) { Rangehat = Phi_0[i + 4]; Bearinghat = Phi_0[i]; Phi_2 = Phi_0[i + 8]; Phi_3 = Phi_0[i + 12]; r = 0.0; for (j = 0; j < 4; j++) { P_tmp_0 = (j << 2) + i; RadarTracker_DW.P[P_tmp_0] = ((((real_T)Phi[j + 4] * Rangehat + Bearinghat * (real_T)Phi[j]) + (real_T)Phi[j + 8] * Phi_2) + (real_T)Phi[j + 12] * Phi_3) + Q[P_tmp_0]; r += (real_T)Phi[P_tmp_0] * RadarTracker_DW.xhat[j]; } Phi_1[i] = r; } RadarTracker_DW.xhat[0] = Phi_1[0]; RadarTracker_DW.xhat[1] = Phi_1[1]; RadarTracker_DW.xhat[2] = Phi_1[2]; RadarTracker_DW.xhat[3] = Phi_1[3]; Rangehat = sqrt(RadarTracker_DW.xhat[0] * RadarTracker_DW.xhat[0] + RadarTracker_DW.xhat[2] * RadarTracker_DW.xhat[2]); Bearinghat = rt_atan2d_snf(RadarTracker_DW.xhat[2], RadarTracker_DW.xhat[0]); Phi_2 = sin(Bearinghat); Phi_3 = cos(Bearinghat); M[0] = Phi_3; M[2] = 0.0; M[4] = Phi_2; M[6] = 0.0; M[1] = -Phi_2 / Rangehat; M[3] = 0.0; M[5] = Phi_3 / Rangehat; M[7] = 0.0; _mm_storeu_pd(&RadarTracker_Y.residual[0], _mm_sub_pd(_mm_loadu_pd (&RadarTracker_U.meas[0]), _mm_set_pd(Bearinghat, Rangehat))); for (i = 0; i < 2; i++) { for (j = 0; j < 4; j++) { x_tmp_tmp = (j << 1) + i; x_tmp[j + (i << 2)] = M[x_tmp_tmp]; P_tmp_0 = j << 2; W[x_tmp_tmp] = ((RadarTracker_DW.P[P_tmp_0 + 1] * 0.0 + RadarTracker_DW.P[P_tmp_0] * M[i]) + RadarTracker_DW.P[P_tmp_0 + 2] * M[i + 4]) + RadarTracker_DW.P[P_tmp_0 + 3] * 0.0; } } for (i = 0; i < 2; i++) { Rangehat = W[i + 2]; Bearinghat = W[i]; Phi_2 = W[i + 4]; Phi_3 = W[i + 6]; for (j = 0; j <= 0; j += 2) { x_tmp_tmp = (j + 1) << 2; P_tmp_0 = j << 2; tmp_4 = (j << 1) + i; tmp_5 = ((j + 1) << 1) + i; _mm_storeu_pd(&tmp_3[0], _mm_add_pd(_mm_add_pd(_mm_add_pd(_mm_add_pd (_mm_mul_pd(_mm_set_pd(x_tmp[x_tmp_tmp + 1], x_tmp[P_tmp_0 + 1]), _mm_set1_pd(Rangehat)), _mm_mul_pd(_mm_set_pd (x_tmp[x_tmp_tmp], x_tmp[P_tmp_0]), _mm_set1_pd(Bearinghat))), _mm_mul_pd(_mm_set_pd(x_tmp[x_tmp_tmp + 2], x_tmp[P_tmp_0 + 2]), _mm_set1_pd(Phi_2))), _mm_mul_pd(_mm_set_pd(x_tmp[x_tmp_tmp + 3], x_tmp[P_tmp_0 + 3]), _mm_set1_pd(Phi_3))), _mm_set_pd(c_b[tmp_5], c_b[tmp_4]))); Phi_1[tmp_4] = tmp_3[0]; Phi_1[tmp_5] = tmp_3[1]; } } if (fabs(Phi_1[1]) > fabs(Phi_1[0])) { r = Phi_1[0] / Phi_1[1]; Rangehat = 1.0 / (r * Phi_1[3] - Phi_1[2]); Bearinghat = Phi_1[3] / Phi_1[1] * Rangehat; Phi_2 = -Rangehat; Phi_3 = -Phi_1[2] / Phi_1[1] * Rangehat; Rangehat *= r; } else { r = Phi_1[1] / Phi_1[0]; Rangehat = 1.0 / (Phi_1[3] - r * Phi_1[2]); Bearinghat = Phi_1[3] / Phi_1[0] * Rangehat; Phi_2 = -r * Rangehat; Phi_3 = -Phi_1[2] / Phi_1[0] * Rangehat; } for (i = 0; i < 4; i++) { r = RadarTracker_DW.P[i + 4]; P = RadarTracker_DW.P[i]; P_0 = RadarTracker_DW.P[i + 8]; P_1 = RadarTracker_DW.P[i + 12]; for (j = 0; j <= 0; j += 2) { x_tmp_tmp = (j + 1) << 2; P_tmp_0 = j << 2; _mm_storeu_pd(&tmp_3[0], _mm_add_pd(_mm_add_pd(_mm_add_pd(_mm_mul_pd (_mm_set_pd(x_tmp[x_tmp_tmp + 1], x_tmp[P_tmp_0 + 1]), _mm_set1_pd(r)), _mm_mul_pd(_mm_set_pd(x_tmp[x_tmp_tmp], x_tmp[P_tmp_0]), _mm_set1_pd(P))), _mm_mul_pd(_mm_set_pd(x_tmp[x_tmp_tmp + 2], x_tmp[P_tmp_0 + 2]), _mm_set1_pd(P_0))), _mm_mul_pd(_mm_set_pd(x_tmp[x_tmp_tmp + 3], x_tmp[P_tmp_0 + 3]), _mm_set1_pd(P_1)))); tmp[i + P_tmp_0] = tmp_3[0]; tmp[i + x_tmp_tmp] = tmp_3[1]; } P = tmp[i + 4]; P_0 = tmp[i]; r = P * Phi_2 + P_0 * Bearinghat; W[i] = r; P = P * Rangehat + P_0 * Phi_3; W[i + 4] = P; RadarTracker_DW.xhat[i] += r * RadarTracker_Y.residual[0] + P * RadarTracker_Y.residual[1]; } for (i = 0; i < 16; i++) { Phi[i] = 0; } Phi[0] = 1; Phi[5] = 1; Phi[10] = 1; Phi[15] = 1; memset(&Q[0], 0, sizeof(real_T) << 4U); for (j = 0; j < 4; j++) { Q[j + (j << 2)] = 1.0; } for (i = 0; i < 4; i++) { P_tmp_0 = i << 1; Rangehat = M[P_tmp_0 + 1]; Bearinghat = M[P_tmp_0]; for (j = 0; j <= 2; j += 2) { tmp_1 = _mm_loadu_pd(&W[j + 4]); tmp_2 = _mm_loadu_pd(&W[j]); _mm_storeu_pd(&P_tmp[j + (i << 2)], _mm_add_pd(_mm_mul_pd(_mm_set1_pd (Rangehat), tmp_1), _mm_mul_pd(_mm_set1_pd(Bearinghat), tmp_2))); } } for (i = 0; i <= 14; i += 2) { tmp_1 = _mm_loadu_pd(&Q[i]); tmp_2 = _mm_loadu_pd(&P_tmp[i]); _mm_storeu_pd(&Q_0[i], _mm_sub_pd(tmp_1, tmp_2)); } for (i = 0; i < 4; i++) { P_tmp_0 = i << 2; r = RadarTracker_DW.P[P_tmp_0 + 1]; P = RadarTracker_DW.P[P_tmp_0]; P_0 = RadarTracker_DW.P[P_tmp_0 + 2]; P_1 = RadarTracker_DW.P[P_tmp_0 + 3]; for (j = 0; j <= 2; j += 2) { tmp_1 = _mm_add_pd(_mm_add_pd(_mm_add_pd(_mm_mul_pd(_mm_set1_pd(r), _mm_loadu_pd(&Q_0[j + 4])), _mm_mul_pd(_mm_set1_pd(P), _mm_loadu_pd (&Q_0[j]))), _mm_mul_pd(_mm_set1_pd(P_0), _mm_loadu_pd(&Q_0[j + 8]))), _mm_mul_pd(_mm_set1_pd(P_1), _mm_loadu_pd(&Q_0[j + 12]))); x_tmp_tmp = j + P_tmp_0; _mm_storeu_pd(&Q[x_tmp_tmp], tmp_1); tmp_1 = _mm_sub_pd(_mm_set_pd(Phi[x_tmp_tmp + 1], Phi[x_tmp_tmp]), _mm_loadu_pd(&P_tmp[x_tmp_tmp])); _mm_storeu_pd(&tmp_3[0], tmp_1); Phi_0[i + (j << 2)] = tmp_3[0]; Phi_0[i + ((j + 1) << 2)] = tmp_3[1]; } } for (j = 0; j <= 2; j += 2) { tmp_1 = _mm_loadu_pd(&W[j + 4]); tmp_2 = _mm_set1_pd(0.0); tmp_0 = _mm_loadu_pd(&W[j]); _mm_storeu_pd(&M[j], _mm_add_pd(_mm_mul_pd(tmp_1, tmp_2), _mm_mul_pd(tmp_0, _mm_set1_pd(90000.0)))); _mm_storeu_pd(&M[j + 4], _mm_add_pd(_mm_mul_pd(tmp_1, _mm_set1_pd(1.0E-6)), _mm_mul_pd(tmp_0, tmp_2))); } for (i = 0; i < 4; i++) { Rangehat = Q[i + 4]; Bearinghat = Q[i]; Phi_2 = Q[i + 8]; Phi_3 = Q[i + 12]; r = M[i + 4]; P = M[i]; for (j = 0; j <= 2; j += 2) { x_tmp_tmp = (j + 1) << 2; P_tmp_0 = j << 2; _mm_storeu_pd(&tmp_3[0], _mm_add_pd(_mm_add_pd(_mm_add_pd(_mm_mul_pd (_mm_set_pd(Phi_0[x_tmp_tmp + 1], Phi_0[P_tmp_0 + 1]), _mm_set1_pd (Rangehat)), _mm_mul_pd(_mm_set_pd(Phi_0[x_tmp_tmp], Phi_0[P_tmp_0]), _mm_set1_pd(Bearinghat))), _mm_mul_pd(_mm_set_pd(Phi_0[x_tmp_tmp + 2], Phi_0[P_tmp_0 + 2]), _mm_set1_pd(Phi_2))), _mm_mul_pd(_mm_set_pd (Phi_0[x_tmp_tmp + 3], Phi_0[P_tmp_0 + 3]), _mm_set1_pd(Phi_3)))); P_tmp_0 += i; Q_0[P_tmp_0] = tmp_3[0]; x_tmp_tmp += i; Q_0[x_tmp_tmp] = tmp_3[1]; tmp_1 = _mm_add_pd(_mm_mul_pd(_mm_set1_pd(r), _mm_loadu_pd(&W[j + 4])), _mm_mul_pd(_mm_set1_pd(P), _mm_loadu_pd(&W[j]))); _mm_storeu_pd(&tmp_3[0], tmp_1); P_tmp[P_tmp_0] = tmp_3[0]; P_tmp[x_tmp_tmp] = tmp_3[1]; } } for (i = 0; i <= 14; i += 2) { tmp_1 = _mm_loadu_pd(&Q_0[i]); tmp_2 = _mm_loadu_pd(&P_tmp[i]); _mm_storeu_pd(&RadarTracker_DW.P[i], _mm_add_pd(tmp_1, tmp_2)); } /* Outport: '<Root>/xhatOut' incorporates: * MATLAB Function: '<Root>/RadarTracker' */ RadarTracker_Y.xhatOut[0] = RadarTracker_DW.xhat[0]; RadarTracker_Y.xhatOut[1] = RadarTracker_DW.xhat[1]; RadarTracker_Y.xhatOut[2] = RadarTracker_DW.xhat[2]; RadarTracker_Y.xhatOut[3] = RadarTracker_DW.xhat[3]; }
You can view the entire generated code in a detailed HTML report, with bi-directional traceability between model and code.
web(fullfile(pwd,'RadarTracker_ert_rtw','html','index.html'))