飞机位置雷达模型
此模型显示为包含 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'))
You can also select a web site from the following list:
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)