Main Content

Aircraft Position Radar Model

This model shows the code generated for a Simulink® model containing a MATLAB® script.

The model contains an Extended Kalman Filter that estimates aircraft position from radar measurements. The MATLAB script AircraftPositionData.m contains data for running the model. The estimated and actual positions are saved to the workspace and are plotted at the end of the simulation by the program AircraftPositionPlot.m (called from the simulation automatically).

Review and Simulate the Model

Review the model and perform a simulation.

Open the Simulink model.

model='AircraftPositionRadar';
open_system(model)
AircraftPositionRadar([],[],[],'compile');
AircraftPositionRadar([],[],[],'term');

Open the MATLAB Function block RadarTracker in the MATLAB Editor.

open_system([model,'/RadarTracker'])

Simulate the model and review the results.

sim(model)

Generate Code for the Model

Generate code for the Kalman Filter portion of the model using the subsystem build functionality provided by Simulink Coder. In the first build, the model is configured to generate code using Simulink Coder™. In the second build, the model is configured to generate code using Embedded Coder®.

Configure and build the model using 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 20.695s

Configure and build the model using 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 18.144s

A portion of RadarTracker.c is listed below.

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];
}

You can view the entire generated code in a detailed HTML report, with bi-directional traceability between model and code.

web(fullfile(pwd,'RadarTracker_ert_rtw','html','index.html'))