Модель радара положения самолета
Эта модель показывает код, сгенерированный для модели Simulink, содержащей скрипт MATLAB.
Модель содержит Расширенный фильтр Калмана, который оценивает положение самолета по радиолокационным измерениям. Скрипт MATLAB rtwdemo_eml_aero_radar.m содержит данные для выполнения модели. Предполагаемые и фактические позиции сохраняются в рабочей области и строятся в конце симуляции программным rtwdemo_aero_radplot (автоматически вызывается из симуляции).
Просмотрите и симулируйте модель
В этом разделе вы должны просмотреть модель и выполнить симуляцию.
Откройте модель Simulink.
model='rtwdemo_eml_aero_radar'; open_system(model) rtwdemo_eml_aero_radar([],[],[],'compile'); rtwdemo_eml_aero_radar([],[],[],'term');
Откройте блок MATLAB Function RadarTracker в редакторе MATLAB.
open_system([model,'/RadarTracker'])
Симулируйте модель и просмотрите результаты (отображаются автоматически).
sim(model)
Сгенерируйте код для модели
В этом разделе вы сгенерируете код для фрагмента модели Фильтр Калмана с помощью функциональности сборки подсистемы, предоставляемой Simulink Coder. В первой сборке модель сконфигурирована для генерации кода с помощью Simulink Coder. Во второй сборке модель сконфигурирована для генерации кода с помощью Embedded Coder.
% Create a temporary folder (in your system's temporary folder) for the % build and inspection process. currentDir = pwd; [~,cgDir] = rtwdemodir();
Сконфигурируйте и создайте модель с помощью Simulink Coder.
rtwconfiguredemo(model,'GRT') 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 10.019s
Сконфигурируйте и создайте модель с помощью Embedded Coder.
rtwconfiguredemo(model,'ERT') 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 11.141s
A фрагмента RadarTracker.c приведен ниже.
cfile = fullfile(cgDir,'RadarTracker_ert_rtw','RadarTracker.c'); rtwdemodbtype(cfile,'/* Model step', '/* Model initialize', 1, 0);
/* Model step function */
void RadarTracker_step(void)
{
real_T P_d_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 Bearinghat;
real_T M_tmp;
real_T M_tmp_0;
real_T Rangehat;
real_T r;
real_T tmp_0;
int32_T Phi_tmp;
int32_T Phi_tmp_tmp;
int32_T i;
int32_T j;
int8_T Phi[16];
static const real_T e[4] = { 0.0, 0.005, 0.0, 0.005 };
static const real_T R[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++) {
Phi_tmp_tmp = i << 2;
Phi_tmp = j + Phi_tmp_tmp;
Phi_0[Phi_tmp] = 0.0;
Phi_0[Phi_tmp] += rtDW.P_d[Phi_tmp_tmp] * (real_T)Phi[j];
Phi_0[Phi_tmp] += rtDW.P_d[Phi_tmp_tmp + 1] * (real_T)Phi[j + 4];
Phi_0[Phi_tmp] += rtDW.P_d[Phi_tmp_tmp + 2] * (real_T)Phi[j + 8];
Phi_0[Phi_tmp] += rtDW.P_d[Phi_tmp_tmp + 3] * (real_T)Phi[j + 12];
}
}
for (i = 0; i < 4; i++) {
Phi_1[i] = 0.0;
for (j = 0; j < 4; j++) {
Phi_tmp_tmp = (j << 2) + i;
rtDW.P_d[Phi_tmp_tmp] = (((Phi_0[i + 4] * (real_T)Phi[j + 4] + Phi_0[i] *
(real_T)Phi[j]) + Phi_0[i + 8] * (real_T)Phi[j + 8]) + Phi_0[i + 12] *
(real_T)Phi[j + 12]) + Q[Phi_tmp_tmp];
Phi_1[i] += (real_T)Phi[Phi_tmp_tmp] * rtDW.xhat[j];
}
}
rtDW.xhat[0] = Phi_1[0];
rtDW.xhat[1] = Phi_1[1];
rtDW.xhat[2] = Phi_1[2];
rtDW.xhat[3] = Phi_1[3];
Rangehat = sqrt(rtDW.xhat[0] * rtDW.xhat[0] + rtDW.xhat[2] * rtDW.xhat[2]);
Bearinghat = atan2(rtDW.xhat[2], rtDW.xhat[0]);
M_tmp = sin(Bearinghat);
M_tmp_0 = cos(Bearinghat);
M[0] = M_tmp_0;
M[2] = 0.0;
M[4] = M_tmp;
M[6] = 0.0;
M[1] = -M_tmp / Rangehat;
M[3] = 0.0;
M[5] = M_tmp_0 / Rangehat;
M[7] = 0.0;
rtY.residual[0] = rtU.meas[0] - Rangehat;
rtY.residual[1] = rtU.meas[1] - Bearinghat;
for (i = 0; i < 2; i++) {
for (j = 0; j < 4; j++) {
Phi_tmp_tmp = (j << 1) + i;
x_tmp[j + (i << 2)] = M[Phi_tmp_tmp];
W[Phi_tmp_tmp] = 0.0;
Phi_tmp = j << 2;
W[Phi_tmp_tmp] += rtDW.P_d[Phi_tmp] * M[i];
W[Phi_tmp_tmp] += rtDW.P_d[Phi_tmp + 2] * M[i + 4];
}
}
for (i = 0; i < 2; i++) {
for (j = 0; j < 2; j++) {
Phi_tmp_tmp = j << 2;
Phi_tmp = (j << 1) + i;
Phi_1[Phi_tmp] = (((x_tmp[Phi_tmp_tmp + 1] * W[i + 2] + x_tmp[Phi_tmp_tmp]
* W[i]) + x_tmp[Phi_tmp_tmp + 2] * W[i + 4]) +
x_tmp[Phi_tmp_tmp + 3] * W[i + 6]) + R[Phi_tmp];
}
}
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;
M_tmp = -Rangehat;
M_tmp_0 = -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;
M_tmp = -r * Rangehat;
M_tmp_0 = -Phi_1[2] / Phi_1[0] * Rangehat;
}
for (i = 0; i < 4; i++) {
for (j = 0; j < 2; j++) {
Phi_tmp_tmp = j << 2;
Phi_tmp = i + Phi_tmp_tmp;
tmp[Phi_tmp] = 0.0;
tmp[Phi_tmp] += x_tmp[Phi_tmp_tmp] * rtDW.P_d[i];
tmp[Phi_tmp] += x_tmp[Phi_tmp_tmp + 1] * rtDW.P_d[i + 4];
tmp[Phi_tmp] += x_tmp[Phi_tmp_tmp + 2] * rtDW.P_d[i + 8];
tmp[Phi_tmp] += x_tmp[Phi_tmp_tmp + 3] * rtDW.P_d[i + 12];
}
W[i] = 0.0;
W[i] += tmp[i] * Bearinghat;
r = tmp[i + 4];
W[i] += r * M_tmp;
tmp_0 = W[i] * rtY.residual[0];
W[i + 4] = 0.0;
W[i + 4] += tmp[i] * M_tmp_0;
W[i + 4] += r * Rangehat;
rtDW.xhat[i] += W[i + 4] * rtY.residual[1] + tmp_0;
}
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++) {
Phi_tmp_tmp = j + (i << 2);
P_d_tmp[Phi_tmp_tmp] = 0.0;
Phi_tmp = i << 1;
P_d_tmp[Phi_tmp_tmp] += M[Phi_tmp] * W[j];
P_d_tmp[Phi_tmp_tmp] += M[Phi_tmp + 1] * W[j + 4];
}
}
for (i = 0; i < 16; i++) {
Q_0[i] = Q[i] - P_d_tmp[i];
}
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++) {
Phi_tmp_tmp = j << 2;
Phi_tmp = i + Phi_tmp_tmp;
Q[Phi_tmp] = 0.0;
Q[Phi_tmp] += rtDW.P_d[Phi_tmp_tmp] * Q_0[i];
Q[Phi_tmp] += rtDW.P_d[Phi_tmp_tmp + 1] * Q_0[i + 4];
Q[Phi_tmp] += rtDW.P_d[Phi_tmp_tmp + 2] * Q_0[i + 8];
Q[Phi_tmp] += rtDW.P_d[Phi_tmp_tmp + 3] * Q_0[i + 12];
Phi_0[j + (i << 2)] = (real_T)Phi[Phi_tmp] - P_d_tmp[Phi_tmp];
}
M[i] = 0.0;
M[i] += W[i] * 90000.0;
M[i + 4] = 0.0;
M[i + 4] += W[i + 4] * 1.0E-6;
}
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++) {
Phi_tmp_tmp = j << 2;
Phi_tmp = i + Phi_tmp_tmp;
Q_0[Phi_tmp] = 0.0;
Q_0[Phi_tmp] += Phi_0[Phi_tmp_tmp] * Q[i];
Q_0[Phi_tmp] += Phi_0[Phi_tmp_tmp + 1] * Q[i + 4];
Q_0[Phi_tmp] += Phi_0[Phi_tmp_tmp + 2] * Q[i + 8];
Q_0[Phi_tmp] += Phi_0[Phi_tmp_tmp + 3] * Q[i + 12];
P_d_tmp[Phi_tmp] = 0.0;
P_d_tmp[Phi_tmp] += M[i] * W[j];
P_d_tmp[Phi_tmp] += M[i + 4] * W[j + 4];
}
}
for (i = 0; i < 16; i++) {
rtDW.P_d[i] = Q_0[i] + P_d_tmp[i];
}
/* Outport: '<Root>/xhatOut' incorporates:
* MATLAB Function: '<Root>/RadarTracker'
*/
rtY.xhatOut[0] = rtDW.xhat[0];
rtY.xhatOut[1] = rtDW.xhat[1];
rtY.xhatOut[2] = rtDW.xhat[2];
rtY.xhatOut[3] = rtDW.xhat[3];
}
Можно просмотреть весь сгенерированный код в подробном HTML с двусторонней отслеживаемостью между моделью и кодом.
web(fullfile(cgDir,'RadarTracker_ert_rtw','html','RadarTracker_codegen_rpt.html'))
Закройте модель и очистите.
bdclose(model) rtwdemoclean; cd(currentDir)