飞机位置雷达模型
此模型显示为包含 MATLAB® 脚本的 Simulink® 模型生成的代码。
该模型包含一个扩展卡尔曼滤波器,用于根据雷达测量值估计飞机位置。MATLAB 脚本 AircraftPositionData.m 包含用于运行模型的数据。估计位置和实际位置将保存到工作区,并在仿真结束时由程序 AircraftPositionPlot.m(自动从仿真中调用)进行绘图。
查看和仿真模型
查看模型并执行仿真。
打开 Simulink 模型。
model='AircraftPositionRadar'; open_system(model) AircraftPositionRadar([],[],[],'compile'); AircraftPositionRadar([],[],[],'term');
在 MATLAB 编辑器中打开 MATLAB Function 模块 RadarTracker
。
open_system([model,'/RadarTracker'])
仿真模型并查看结果。
sim(model)
为模型生成代码
使用 Simulink Coder 提供的子系统编译功能为模型的卡尔曼滤波器部分生成代码。在第一个编译中,该模型配置为使用 Simulink Coder™ 生成代码。在第二个编译中,该模型配置为使用 Embedded Coder® 生成代码。
使用 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 built: Model Action Rebuild Reason ============================================================================================== RadarTracker Code generated and compiled. Code generation information file does not exist. 1 of 1 models built (0 models already up to date) Build duration: 0h 0m 21.179s
使用 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 built: Model Action Rebuild Reason ============================================================================================== RadarTracker Code generated and compiled. Code generation information file does not exist. 1 of 1 models built (0 models already up to date) Build duration: 0h 0m 17.878s
下面列出了 RadarTracker.c
的一部分。
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]; }
您可以在详细的 HTML 报告中查看生成的完整代码,该报告可在模型和代码之间提供双向可追溯性。
web(fullfile(pwd,'RadarTracker_ert_rtw','html','index.html'))