Радарная модель положения самолета

Эта модель показывает код, сгенерированный для модели 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')
rtwbuild([model,'/RadarTracker'])
### Starting build procedure for model: RadarTracker
### Successful completion of build procedure for model: RadarTracker

Сконфигурируйте и создайте модель с помощью Embedded Coder.

rtwconfiguredemo(model,'ERT')
rtwbuild([model,'/RadarTracker'])
### Starting build procedure for model: RadarTracker
### Successful completion of build procedure for model: RadarTracker

Фрагмент 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)
{
  int8_T Phi[16];
  real_T Q[16];
  real_T Rangehat;
  real_T Bearinghat;
  real_T M[8];
  real_T W[8];
  int32_T j;
  real_T r;
  static const real_T d[4] = { 0.0, 0.005, 0.0, 0.005 };

  static const real_T R[4] = { 90000.0, 0.0, 0.0, 1.0E-6 };

  real_T x_tmp[8];
  real_T P_a_tmp[16];
  real_T Phi_0[16];
  real_T Phi_1[4];
  real_T M_0[8];
  real_T Phi_2[16];
  real_T Q_0[16];
  int32_T i;
  int32_T Phi_tmp;
  int32_T Phi_tmp_tmp;
  real_T M_tmp;
  real_T M_tmp_0;

  /* 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)] = d[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_a[Phi_tmp_tmp] * (real_T)Phi[j];
      Phi_0[Phi_tmp] += rtDW.P_a[Phi_tmp_tmp + 1] * (real_T)Phi[j + 4];
      Phi_0[Phi_tmp] += rtDW.P_a[Phi_tmp_tmp + 2] * (real_T)Phi[j + 8];
      Phi_0[Phi_tmp] += rtDW.P_a[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_a[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_0 = cos(Bearinghat);
  M[0] = M_tmp_0;
  M[2] = 0.0;
  M_tmp = sin(Bearinghat);
  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];
      M_0[Phi_tmp_tmp] = 0.0;
      Phi_tmp = j << 2;
      M_0[Phi_tmp_tmp] += rtDW.P_a[Phi_tmp] * M[i];
      M_0[Phi_tmp_tmp] += rtDW.P_a[Phi_tmp + 2] * M[i + 4];
    }
  }

  for (i = 0; i < 2; i++) {
    for (j = 0; j < 2; j++) {
      Phi_tmp_tmp = i << 2;
      Phi_tmp = (i << 1) + j;
      Phi_1[Phi_tmp] = (((x_tmp[Phi_tmp_tmp + 1] * M_0[j + 2] +
                          x_tmp[Phi_tmp_tmp] * M_0[j]) + x_tmp[Phi_tmp_tmp + 2] *
                         M_0[j + 4]) + x_tmp[Phi_tmp_tmp + 3] * M_0[j + 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_0 = -Rangehat;
    M_tmp = -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_0 = -r * Rangehat;
    M_tmp = -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;
      M_0[Phi_tmp] = 0.0;
      M_0[Phi_tmp] += x_tmp[Phi_tmp_tmp] * rtDW.P_a[i];
      M_0[Phi_tmp] += x_tmp[Phi_tmp_tmp + 1] * rtDW.P_a[i + 4];
      M_0[Phi_tmp] += x_tmp[Phi_tmp_tmp + 2] * rtDW.P_a[i + 8];
      M_0[Phi_tmp] += x_tmp[Phi_tmp_tmp + 3] * rtDW.P_a[i + 12];
    }

    W[i] = 0.0;
    W[i] += M_0[i] * Bearinghat;
    W[i] += M_0[i + 4] * M_tmp_0;
    r = W[i] * rtY.residual[0];
    W[i + 4] = 0.0;
    W[i + 4] += M_0[i] * M_tmp;
    W[i + 4] += M_0[i + 4] * Rangehat;
    r += W[i + 4] * rtY.residual[1];
    rtDW.xhat[i] += r;
  }

  memset(&Q[0], 0, sizeof(real_T) << 4U);
  for (i = 0; i < 16; i++) {
    Phi[i] = 0;
  }

  Phi[0] = 1;
  Phi[5] = 1;
  Phi[10] = 1;
  Phi[15] = 1;
  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_a_tmp[Phi_tmp_tmp] = 0.0;
      Phi_tmp = i << 1;
      P_a_tmp[Phi_tmp_tmp] += M[Phi_tmp] * W[j];
      P_a_tmp[Phi_tmp_tmp] += M[Phi_tmp + 1] * W[j + 4];
    }
  }

  for (i = 0; i < 16; i++) {
    Phi_0[i] = (real_T)Phi[i] - P_a_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;
      Phi_2[Phi_tmp] = 0.0;
      Phi_2[Phi_tmp] += rtDW.P_a[Phi_tmp_tmp] * Phi_0[i];
      Phi_2[Phi_tmp] += rtDW.P_a[Phi_tmp_tmp + 1] * Phi_0[i + 4];
      Phi_2[Phi_tmp] += rtDW.P_a[Phi_tmp_tmp + 2] * Phi_0[i + 8];
      Phi_2[Phi_tmp] += rtDW.P_a[Phi_tmp_tmp + 3] * Phi_0[i + 12];
      Q_0[j + (i << 2)] = Q[Phi_tmp] - P_a_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;
      Phi_0[Phi_tmp] = 0.0;
      Phi_0[Phi_tmp] += Q_0[Phi_tmp_tmp] * Phi_2[i];
      Phi_0[Phi_tmp] += Q_0[Phi_tmp_tmp + 1] * Phi_2[i + 4];
      Phi_0[Phi_tmp] += Q_0[Phi_tmp_tmp + 2] * Phi_2[i + 8];
      Phi_0[Phi_tmp] += Q_0[Phi_tmp_tmp + 3] * Phi_2[i + 12];
      Q[Phi_tmp] = 0.0;
      Q[Phi_tmp] += M[i] * W[j];
      Q[Phi_tmp] += M[i + 4] * W[j + 4];
    }
  }

  for (i = 0; i < 16; i++) {
    rtDW.P_a[i] = Phi_0[i] + Q[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)