飞机位置雷达模型
此模型显示为包含 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'])
### Searching for referenced models in model 'RadarTracker'. ### Total of 1 models to build. ### 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 11.338s 1 of 1 models built (0 models already up to date) Build duration: 0h 0m 11.521s
使用 Embedded Coder 配置和编译模型。
set_param(model, "SystemTargetFile", "ert.tlc"); slbuild([model,'/RadarTracker'])
### Searching for referenced models in model 'RadarTracker'. ### Total of 1 models to build. ### 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 11.771s 1 of 1 models built (0 models already up to date) Build duration: 0h 0m 12.017s
下面列出了 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; __m128d tmp_3; __m128d tmp_5; 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 b[4]; real_T x[4]; real_T tmp_4[2]; real_T Bearinghat; real_T Phi_5; real_T Phi_6; real_T Rangehat; int32_T Q_tmp; int32_T i; int32_T j; int32_T tmp_6; int32_T tmp_7; int32_T x_tmp_tmp; int8_T Phi[16]; int8_T Phi_1; int8_T Phi_2; int8_T Phi_3; int8_T Phi_4; 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_tmp = j << 2; Q[j + Q_tmp] = e[j]; Rangehat = 0.0; Bearinghat = 0.0; Phi_5 = 0.0; Phi_6 = 0.0; for (i = 0; i < 4; i++) { x_tmp_tmp = i << 2; tmp_5 = _mm_set1_pd(RadarTracker_DW.P[Q_tmp + i]); _mm_storeu_pd(&tmp_4[0], _mm_add_pd(_mm_mul_pd(_mm_set_pd(Phi[x_tmp_tmp + 1], Phi[x_tmp_tmp]), tmp_5), _mm_set_pd(Bearinghat, Rangehat))); Rangehat = tmp_4[0]; Bearinghat = tmp_4[1]; _mm_storeu_pd(&tmp_4[0], _mm_add_pd(_mm_mul_pd(_mm_set_pd(Phi[x_tmp_tmp + 3], Phi[x_tmp_tmp + 2]), tmp_5), _mm_set_pd(Phi_6, Phi_5))); Phi_5 = tmp_4[0]; Phi_6 = tmp_4[1]; } Phi_0[Q_tmp + 3] = Phi_6; Phi_0[Q_tmp + 2] = Phi_5; Phi_0[Q_tmp + 1] = Bearinghat; Phi_0[Q_tmp] = Rangehat; } Rangehat = 0.0; Bearinghat = 0.0; Phi_5 = 0.0; Phi_6 = 0.0; for (i = 0; i < 4; i++) { Phi_1 = Phi[i + 4]; Phi_2 = Phi[i]; Phi_3 = Phi[i + 8]; Phi_4 = Phi[i + 12]; for (x_tmp_tmp = 0; x_tmp_tmp <= 2; x_tmp_tmp += 2) { tmp_5 = _mm_loadu_pd(&Phi_0[x_tmp_tmp + 4]); tmp_0 = _mm_loadu_pd(&Phi_0[x_tmp_tmp]); tmp_1 = _mm_loadu_pd(&Phi_0[x_tmp_tmp + 8]); tmp_2 = _mm_loadu_pd(&Phi_0[x_tmp_tmp + 12]); j = (i << 2) + x_tmp_tmp; tmp_3 = _mm_loadu_pd(&Q[j]); _mm_storeu_pd(&RadarTracker_DW.P[j], _mm_add_pd(_mm_add_pd(_mm_add_pd (_mm_add_pd(_mm_mul_pd(tmp_5, _mm_set1_pd(Phi_1)), _mm_mul_pd(tmp_0, _mm_set1_pd(Phi_2))), _mm_mul_pd(tmp_1, _mm_set1_pd(Phi_3))), _mm_mul_pd (tmp_2, _mm_set1_pd(Phi_4))), tmp_3)); } x_tmp_tmp = i << 2; tmp_5 = _mm_set1_pd(RadarTracker_DW.xhat[i]); _mm_storeu_pd(&tmp_4[0], _mm_add_pd(_mm_mul_pd(_mm_set_pd(Phi[x_tmp_tmp + 1], Phi[x_tmp_tmp]), tmp_5), _mm_set_pd(Bearinghat, Rangehat))); Rangehat = tmp_4[0]; Bearinghat = tmp_4[1]; _mm_storeu_pd(&tmp_4[0], _mm_add_pd(_mm_mul_pd(_mm_set_pd(Phi[x_tmp_tmp + 3], Phi[x_tmp_tmp + 2]), tmp_5), _mm_set_pd(Phi_6, Phi_5))); Phi_5 = tmp_4[0]; Phi_6 = tmp_4[1]; } RadarTracker_DW.xhat[0] = Rangehat; RadarTracker_DW.xhat[1] = Bearinghat; RadarTracker_DW.xhat[2] = Phi_5; RadarTracker_DW.xhat[3] = Phi_6; 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_5 = sin(Bearinghat); Phi_6 = cos(Bearinghat); M[0] = Phi_6; M[2] = 0.0; M[4] = Phi_5; M[6] = 0.0; M[1] = -Phi_5 / Rangehat; M[3] = 0.0; M[5] = Phi_6 / 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++) { x_tmp_tmp = i << 2; x_tmp[x_tmp_tmp] = M[i]; x_tmp[x_tmp_tmp + 1] = 0.0; x_tmp[x_tmp_tmp + 2] = M[i + 4]; x_tmp[x_tmp_tmp + 3] = 0.0; } for (i = 0; i < 4; i++) { Rangehat = 0.0; Bearinghat = 0.0; for (x_tmp_tmp = 0; x_tmp_tmp < 4; x_tmp_tmp++) { tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&M[x_tmp_tmp << 1]), _mm_set1_pd(RadarTracker_DW.P[(i << 2) + x_tmp_tmp])), _mm_set_pd (Bearinghat, Rangehat)); _mm_storeu_pd(&tmp_4[0], tmp_5); Rangehat = tmp_4[0]; Bearinghat = tmp_4[1]; } x_tmp_tmp = i << 1; W[x_tmp_tmp + 1] = Bearinghat; W[x_tmp_tmp] = Rangehat; } for (i = 0; i < 2; i++) { Rangehat = W[i + 2]; Bearinghat = W[i]; Phi_5 = W[i + 4]; Phi_6 = W[i + 6]; for (x_tmp_tmp = 0; x_tmp_tmp <= 0; x_tmp_tmp += 2) { j = (x_tmp_tmp + 1) << 2; Q_tmp = x_tmp_tmp << 2; tmp_6 = (x_tmp_tmp << 1) + i; tmp_7 = ((x_tmp_tmp + 1) << 1) + i; _mm_storeu_pd(&tmp_4[0], _mm_add_pd(_mm_add_pd(_mm_add_pd(_mm_add_pd (_mm_mul_pd(_mm_set_pd(x_tmp[j + 1], x_tmp[Q_tmp + 1]), _mm_set1_pd (Rangehat)), _mm_mul_pd(_mm_set_pd(x_tmp[j], x_tmp[Q_tmp]), _mm_set1_pd(Bearinghat))), _mm_mul_pd(_mm_set_pd(x_tmp[j + 2], x_tmp[Q_tmp + 2]), _mm_set1_pd(Phi_5))), _mm_mul_pd(_mm_set_pd(x_tmp[j + 3], x_tmp[Q_tmp + 3]), _mm_set1_pd(Phi_6))), _mm_set_pd(c_b[tmp_7], c_b[tmp_6]))); x[tmp_6] = tmp_4[0]; x[tmp_7] = tmp_4[1]; } } if (fabs(x[1]) > fabs(x[0])) { Rangehat = x[0] / x[1]; Bearinghat = 1.0 / (Rangehat * x[3] - x[2]); b[0] = x[3] / x[1] * Bearinghat; b[1] = -Bearinghat; b[2] = -x[2] / x[1] * Bearinghat; b[3] = Rangehat * Bearinghat; } else { Rangehat = x[1] / x[0]; Bearinghat = 1.0 / (x[3] - Rangehat * x[2]); b[0] = x[3] / x[0] * Bearinghat; b[1] = -Rangehat * Bearinghat; b[2] = -x[2] / x[0] * Bearinghat; b[3] = Bearinghat; } for (i = 0; i < 2; i++) { Rangehat = 0.0; Bearinghat = 0.0; Phi_5 = 0.0; Phi_6 = 0.0; for (x_tmp_tmp = 0; x_tmp_tmp < 4; x_tmp_tmp++) { j = x_tmp_tmp << 2; Q_tmp = (i << 2) + x_tmp_tmp; tmp_5 = _mm_set1_pd(x_tmp[Q_tmp]); tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&RadarTracker_DW.P[j]), tmp_5), _mm_set_pd(Bearinghat, Rangehat)); _mm_storeu_pd(&tmp_4[0], tmp_0); Rangehat = tmp_4[0]; Bearinghat = tmp_4[1]; tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&RadarTracker_DW.P[j + 2]), tmp_5), _mm_set_pd(Phi_6, Phi_5)); _mm_storeu_pd(&tmp_4[0], tmp_5); Phi_5 = tmp_4[0]; Phi_6 = tmp_4[1]; W[Q_tmp] = 0.0; } x_tmp_tmp = i << 2; tmp[x_tmp_tmp + 3] = Phi_6; tmp[x_tmp_tmp + 2] = Phi_5; tmp[x_tmp_tmp + 1] = Bearinghat; tmp[x_tmp_tmp] = Rangehat; } for (i = 0; i < 2; i++) { Q_tmp = i << 2; Rangehat = W[Q_tmp]; Bearinghat = W[Q_tmp + 1]; Phi_5 = W[Q_tmp + 2]; Phi_6 = W[Q_tmp + 3]; for (x_tmp_tmp = 0; x_tmp_tmp < 2; x_tmp_tmp++) { j = x_tmp_tmp << 2; tmp_5 = _mm_set1_pd(b[(i << 1) + x_tmp_tmp]); tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&tmp[j]), tmp_5), _mm_set_pd (Bearinghat, Rangehat)); _mm_storeu_pd(&tmp_4[0], tmp_0); Rangehat = tmp_4[0]; Bearinghat = tmp_4[1]; tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&tmp[j + 2]), tmp_5), _mm_set_pd(Phi_6, Phi_5)); _mm_storeu_pd(&tmp_4[0], tmp_5); Phi_5 = tmp_4[0]; Phi_6 = tmp_4[1]; } W[Q_tmp + 3] = Phi_6; W[Q_tmp + 2] = Phi_5; W[Q_tmp + 1] = Bearinghat; W[Q_tmp] = Rangehat; } Rangehat = RadarTracker_Y.residual[1]; Bearinghat = RadarTracker_Y.residual[0]; for (i = 0; i <= 2; i += 2) { tmp_5 = _mm_loadu_pd(&W[i + 4]); tmp_0 = _mm_loadu_pd(&W[i]); tmp_1 = _mm_loadu_pd(&RadarTracker_DW.xhat[i]); _mm_storeu_pd(&RadarTracker_DW.xhat[i], _mm_add_pd(_mm_add_pd(_mm_mul_pd (tmp_5, _mm_set1_pd(Rangehat)), _mm_mul_pd(tmp_0, _mm_set1_pd(Bearinghat))), tmp_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_tmp = j << 2; Q[j + Q_tmp] = 1.0; Rangehat = 0.0; Bearinghat = 0.0; Phi_5 = 0.0; Phi_6 = 0.0; for (i = 0; i < 2; i++) { x_tmp_tmp = i << 2; tmp_5 = _mm_set1_pd(M[(j << 1) + i]); tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&W[x_tmp_tmp]), tmp_5), _mm_set_pd(Bearinghat, Rangehat)); _mm_storeu_pd(&tmp_4[0], tmp_0); Rangehat = tmp_4[0]; Bearinghat = tmp_4[1]; tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&W[x_tmp_tmp + 2]), tmp_5), _mm_set_pd(Phi_6, Phi_5)); _mm_storeu_pd(&tmp_4[0], tmp_5); Phi_5 = tmp_4[0]; Phi_6 = tmp_4[1]; } P_tmp[Q_tmp + 3] = Phi_6; P_tmp[Q_tmp + 2] = Phi_5; P_tmp[Q_tmp + 1] = Bearinghat; P_tmp[Q_tmp] = Rangehat; } for (i = 0; i <= 14; i += 2) { tmp_5 = _mm_loadu_pd(&Q[i]); tmp_0 = _mm_loadu_pd(&P_tmp[i]); _mm_storeu_pd(&Q_0[i], _mm_sub_pd(tmp_5, tmp_0)); } for (i = 0; i < 4; i++) { Rangehat = 0.0; Bearinghat = 0.0; Phi_5 = 0.0; Phi_6 = 0.0; for (x_tmp_tmp = 0; x_tmp_tmp < 4; x_tmp_tmp++) { j = x_tmp_tmp << 2; Q_tmp = (i << 2) + x_tmp_tmp; tmp_5 = _mm_set1_pd(RadarTracker_DW.P[Q_tmp]); tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&Q_0[j]), tmp_5), _mm_set_pd (Bearinghat, Rangehat)); _mm_storeu_pd(&tmp_4[0], tmp_0); Rangehat = tmp_4[0]; Bearinghat = tmp_4[1]; tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&Q_0[j + 2]), tmp_5), _mm_set_pd(Phi_6, Phi_5)); _mm_storeu_pd(&tmp_4[0], tmp_5); Phi_5 = tmp_4[0]; Phi_6 = tmp_4[1]; j += i; Phi_0[Q_tmp] = (real_T)Phi[j] - P_tmp[j]; } Q_tmp = i << 2; Q[Q_tmp + 3] = Phi_6; Q[Q_tmp + 2] = Phi_5; Q[Q_tmp + 1] = Bearinghat; Q[Q_tmp] = Rangehat; } for (i = 0; i < 2; i++) { Rangehat = 0.0; Bearinghat = 0.0; Phi_5 = 0.0; Phi_6 = 0.0; for (x_tmp_tmp = 0; x_tmp_tmp < 2; x_tmp_tmp++) { j = x_tmp_tmp << 2; tmp_5 = _mm_set1_pd(c_b[(i << 1) + x_tmp_tmp]); tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&W[j]), tmp_5), _mm_set_pd (Bearinghat, Rangehat)); _mm_storeu_pd(&tmp_4[0], tmp_0); Rangehat = tmp_4[0]; Bearinghat = tmp_4[1]; tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&W[j + 2]), tmp_5), _mm_set_pd (Phi_6, Phi_5)); _mm_storeu_pd(&tmp_4[0], tmp_5); Phi_5 = tmp_4[0]; Phi_6 = tmp_4[1]; } Q_tmp = i << 2; M[Q_tmp + 3] = Phi_6; M[Q_tmp + 2] = Phi_5; M[Q_tmp + 1] = Bearinghat; M[Q_tmp] = Rangehat; } for (i = 0; i < 4; i++) { Rangehat = 0.0; Bearinghat = 0.0; Phi_5 = 0.0; Phi_6 = 0.0; for (x_tmp_tmp = 0; x_tmp_tmp < 4; x_tmp_tmp++) { j = x_tmp_tmp << 2; Q_tmp = (i << 2) + x_tmp_tmp; tmp_5 = _mm_set1_pd(Phi_0[Q_tmp]); tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&Q[j]), tmp_5), _mm_set_pd (Bearinghat, Rangehat)); _mm_storeu_pd(&tmp_4[0], tmp_0); Rangehat = tmp_4[0]; Bearinghat = tmp_4[1]; tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&Q[j + 2]), tmp_5), _mm_set_pd (Phi_6, Phi_5)); _mm_storeu_pd(&tmp_4[0], tmp_5); Phi_5 = tmp_4[0]; Phi_6 = tmp_4[1]; P_tmp[Q_tmp] = 0.0; } Q_tmp = i << 2; Q_0[Q_tmp + 3] = Phi_6; Q_0[Q_tmp + 2] = Phi_5; Q_0[Q_tmp + 1] = Bearinghat; Q_0[Q_tmp] = Rangehat; Rangehat = P_tmp[Q_tmp]; Bearinghat = P_tmp[Q_tmp + 1]; Phi_5 = P_tmp[Q_tmp + 2]; Phi_6 = P_tmp[Q_tmp + 3]; for (x_tmp_tmp = 0; x_tmp_tmp < 2; x_tmp_tmp++) { j = x_tmp_tmp << 2; tmp_5 = _mm_set1_pd(W[j + i]); tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&M[j]), tmp_5), _mm_set_pd (Bearinghat, Rangehat)); _mm_storeu_pd(&tmp_4[0], tmp_0); Rangehat = tmp_4[0]; Bearinghat = tmp_4[1]; tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&M[j + 2]), tmp_5), _mm_set_pd (Phi_6, Phi_5)); _mm_storeu_pd(&tmp_4[0], tmp_5); Phi_5 = tmp_4[0]; Phi_6 = tmp_4[1]; } P_tmp[Q_tmp + 3] = Phi_6; P_tmp[Q_tmp + 2] = Phi_5; P_tmp[Q_tmp + 1] = Bearinghat; P_tmp[Q_tmp] = Rangehat; } for (i = 0; i <= 14; i += 2) { tmp_5 = _mm_loadu_pd(&Q_0[i]); tmp_0 = _mm_loadu_pd(&P_tmp[i]); _mm_storeu_pd(&RadarTracker_DW.P[i], _mm_add_pd(tmp_5, tmp_0)); } /* 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'))