Main Content

このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

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

このモデルでは、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™ を使用してコードを生成するように構成されています。2 番目のビルドでは、モデルは 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 15.615s

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

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 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 M_tmp;
  int32_T P_tmp_0;
  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++) {
    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 < 4; j++) {
      Phi_0[j + P_tmp_0] = (((real_T)Phi[j + 4] * r + P * (real_T)Phi[j]) +
                            (real_T)Phi[j + 8] * P_0) + (real_T)Phi[j + 12] *
        P_1;
    }
  }

  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;
  RadarTracker_Y.residual[0] = RadarTracker_U.meas[0] - Rangehat;
  RadarTracker_Y.residual[1] = RadarTracker_U.meas[1] - Bearinghat;
  for (i = 0; i < 2; i++) {
    for (j = 0; j < 4; j++) {
      P_tmp_0 = (j << 1) + i;
      x_tmp[j + (i << 2)] = M[P_tmp_0];
      M_tmp = j << 2;
      W[P_tmp_0] = ((RadarTracker_DW.P[M_tmp + 1] * 0.0 +
                     RadarTracker_DW.P[M_tmp] * M[i]) + RadarTracker_DW.P[M_tmp
                    + 2] * M[i + 4]) + RadarTracker_DW.P[M_tmp + 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 < 2; j++) {
      P_tmp_0 = j << 2;
      M_tmp = (j << 1) + i;
      Phi_1[M_tmp] = (((x_tmp[P_tmp_0 + 1] * Rangehat + x_tmp[P_tmp_0] *
                        Bearinghat) + x_tmp[P_tmp_0 + 2] * Phi_2) +
                      x_tmp[P_tmp_0 + 3] * Phi_3) + c_b[M_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;
    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 < 2; j++) {
      P_tmp_0 = j << 2;
      tmp[i + P_tmp_0] = ((x_tmp[P_tmp_0 + 1] * r + x_tmp[P_tmp_0] * P) +
                          x_tmp[P_tmp_0 + 2] * P_0) + x_tmp[P_tmp_0 + 3] * P_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++) {
    M_tmp = i << 1;
    Rangehat = M[M_tmp + 1];
    Bearinghat = M[M_tmp];
    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) {
    /* MATLAB Function: '<Root>/RadarTracker' */
    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));
  }

  /* MATLAB Function: '<Root>/RadarTracker' */
  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 < 4; j++) {
      M_tmp = j + P_tmp_0;
      Q[M_tmp] = ((Q_0[j + 4] * r + P * Q_0[j]) + Q_0[j + 8] * P_0) + Q_0[j + 12]
        * P_1;
      Phi_0[i + (j << 2)] = (real_T)Phi[M_tmp] - P_tmp[M_tmp];
    }
  }

  for (j = 0; j <= 2; j += 2) {
    /* MATLAB Function: '<Root>/RadarTracker' */
    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)));
  }

  /* MATLAB Function: '<Root>/RadarTracker' */
  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 < 4; j++) {
      M_tmp = j << 2;
      P_tmp_0 = i + M_tmp;
      Q_0[P_tmp_0] = ((Phi_0[M_tmp + 1] * Rangehat + Phi_0[M_tmp] * Bearinghat)
                      + Phi_0[M_tmp + 2] * Phi_2) + Phi_0[M_tmp + 3] * Phi_3;
      P_tmp[P_tmp_0] = W[j + 4] * r + P * W[j];
    }
  }

  for (i = 0; i <= 14; i += 2) {
    /* MATLAB Function: '<Root>/RadarTracker' */
    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'))