航空機の位置レーダーモデル

このモデルでは、MATLAB スクリプトを含む Simulink モデルに生成されたコードを説明します。

モデルには、航空機の位置をレーダーの測定値から推定する拡張カルマンフィルターが含まれています。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 エディターで MATLAB Function ブロック RadarTracker を開きます。

open_system([model,'/RadarTracker'])

モデルをシミュレーションし、結果を確認します (自動的に表示)。

sim(model)

モデルのコード生成

この節では、Simulink Coder が提供するサブシステムのビルド機能を使用してモデルのカルマンフィルター部分のコードを生成します。最初のビルドでは、モデルは Simulink Coder を使用してコードを生成するように構成されています。2 番目のビルドでは、モデルは 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 47.489s

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 25.1s

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 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++) {
      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]) + c_b[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 = (i << 2) + j;
      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','index.html'))

モデルを閉じてクリーンアップします。

bdclose(model)
rtwdemoclean;
cd(currentDir)

航空機の位置レーダー モデル

モデルの確認とシミュレーション

モデルのコード生成

航空機の位置レーダーモデル