このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。
航空機の位置レーダー モデル
このモデルでは、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'])
### Searching for referenced models in model 'RadarTracker'. ### Total of 1 models to build. ### Starting build procedure for: RadarTracker ### Successful completion of build procedure for: RadarTracker Build Summary Top model targets: Model Build Reason Status Build Duration =============================================================================================================== RadarTracker Information cache folder or artifacts were missing. Code generated and compiled. 0h 0m 11.338s 1 of 1 models built (0 models already up to date) Build duration: 0h 0m 11.521s
Embedded Coder を使用してモデルを構成およびビルドします。
set_param(model, "SystemTargetFile", "ert.tlc"); slbuild([model,'/RadarTracker'])
### Searching for referenced models in model 'RadarTracker'. ### Total of 1 models to build. ### Starting build procedure for: RadarTracker ### Successful completion of build procedure for: RadarTracker Build Summary Top model targets: Model Build Reason Status Build Duration =============================================================================================================== RadarTracker Information cache folder or artifacts were missing. Code generated and compiled. 0h 0m 11.771s 1 of 1 models built (0 models already up to date) Build duration: 0h 0m 12.017s
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;
__m128d tmp_3;
__m128d tmp_5;
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 b[4];
real_T x[4];
real_T tmp_4[2];
real_T Bearinghat;
real_T Phi_5;
real_T Phi_6;
real_T Rangehat;
int32_T Q_tmp;
int32_T i;
int32_T j;
int32_T tmp_6;
int32_T tmp_7;
int32_T x_tmp_tmp;
int8_T Phi[16];
int8_T Phi_1;
int8_T Phi_2;
int8_T Phi_3;
int8_T Phi_4;
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_tmp = j << 2;
Q[j + Q_tmp] = e[j];
Rangehat = 0.0;
Bearinghat = 0.0;
Phi_5 = 0.0;
Phi_6 = 0.0;
for (i = 0; i < 4; i++) {
x_tmp_tmp = i << 2;
tmp_5 = _mm_set1_pd(RadarTracker_DW.P[Q_tmp + i]);
_mm_storeu_pd(&tmp_4[0], _mm_add_pd(_mm_mul_pd(_mm_set_pd(Phi[x_tmp_tmp +
1], Phi[x_tmp_tmp]), tmp_5), _mm_set_pd(Bearinghat, Rangehat)));
Rangehat = tmp_4[0];
Bearinghat = tmp_4[1];
_mm_storeu_pd(&tmp_4[0], _mm_add_pd(_mm_mul_pd(_mm_set_pd(Phi[x_tmp_tmp +
3], Phi[x_tmp_tmp + 2]), tmp_5), _mm_set_pd(Phi_6, Phi_5)));
Phi_5 = tmp_4[0];
Phi_6 = tmp_4[1];
}
Phi_0[Q_tmp + 3] = Phi_6;
Phi_0[Q_tmp + 2] = Phi_5;
Phi_0[Q_tmp + 1] = Bearinghat;
Phi_0[Q_tmp] = Rangehat;
}
Rangehat = 0.0;
Bearinghat = 0.0;
Phi_5 = 0.0;
Phi_6 = 0.0;
for (i = 0; i < 4; i++) {
Phi_1 = Phi[i + 4];
Phi_2 = Phi[i];
Phi_3 = Phi[i + 8];
Phi_4 = Phi[i + 12];
for (x_tmp_tmp = 0; x_tmp_tmp <= 2; x_tmp_tmp += 2) {
tmp_5 = _mm_loadu_pd(&Phi_0[x_tmp_tmp + 4]);
tmp_0 = _mm_loadu_pd(&Phi_0[x_tmp_tmp]);
tmp_1 = _mm_loadu_pd(&Phi_0[x_tmp_tmp + 8]);
tmp_2 = _mm_loadu_pd(&Phi_0[x_tmp_tmp + 12]);
j = (i << 2) + x_tmp_tmp;
tmp_3 = _mm_loadu_pd(&Q[j]);
_mm_storeu_pd(&RadarTracker_DW.P[j], _mm_add_pd(_mm_add_pd(_mm_add_pd
(_mm_add_pd(_mm_mul_pd(tmp_5, _mm_set1_pd(Phi_1)), _mm_mul_pd(tmp_0,
_mm_set1_pd(Phi_2))), _mm_mul_pd(tmp_1, _mm_set1_pd(Phi_3))), _mm_mul_pd
(tmp_2, _mm_set1_pd(Phi_4))), tmp_3));
}
x_tmp_tmp = i << 2;
tmp_5 = _mm_set1_pd(RadarTracker_DW.xhat[i]);
_mm_storeu_pd(&tmp_4[0], _mm_add_pd(_mm_mul_pd(_mm_set_pd(Phi[x_tmp_tmp + 1],
Phi[x_tmp_tmp]), tmp_5), _mm_set_pd(Bearinghat, Rangehat)));
Rangehat = tmp_4[0];
Bearinghat = tmp_4[1];
_mm_storeu_pd(&tmp_4[0], _mm_add_pd(_mm_mul_pd(_mm_set_pd(Phi[x_tmp_tmp + 3],
Phi[x_tmp_tmp + 2]), tmp_5), _mm_set_pd(Phi_6, Phi_5)));
Phi_5 = tmp_4[0];
Phi_6 = tmp_4[1];
}
RadarTracker_DW.xhat[0] = Rangehat;
RadarTracker_DW.xhat[1] = Bearinghat;
RadarTracker_DW.xhat[2] = Phi_5;
RadarTracker_DW.xhat[3] = Phi_6;
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_5 = sin(Bearinghat);
Phi_6 = cos(Bearinghat);
M[0] = Phi_6;
M[2] = 0.0;
M[4] = Phi_5;
M[6] = 0.0;
M[1] = -Phi_5 / Rangehat;
M[3] = 0.0;
M[5] = Phi_6 / Rangehat;
M[7] = 0.0;
_mm_storeu_pd(&RadarTracker_Y.residual[0], _mm_sub_pd(_mm_loadu_pd
(&RadarTracker_U.meas[0]), _mm_set_pd(Bearinghat, Rangehat)));
for (i = 0; i < 2; i++) {
x_tmp_tmp = i << 2;
x_tmp[x_tmp_tmp] = M[i];
x_tmp[x_tmp_tmp + 1] = 0.0;
x_tmp[x_tmp_tmp + 2] = M[i + 4];
x_tmp[x_tmp_tmp + 3] = 0.0;
}
for (i = 0; i < 4; i++) {
Rangehat = 0.0;
Bearinghat = 0.0;
for (x_tmp_tmp = 0; x_tmp_tmp < 4; x_tmp_tmp++) {
tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&M[x_tmp_tmp << 1]),
_mm_set1_pd(RadarTracker_DW.P[(i << 2) + x_tmp_tmp])), _mm_set_pd
(Bearinghat, Rangehat));
_mm_storeu_pd(&tmp_4[0], tmp_5);
Rangehat = tmp_4[0];
Bearinghat = tmp_4[1];
}
x_tmp_tmp = i << 1;
W[x_tmp_tmp + 1] = Bearinghat;
W[x_tmp_tmp] = Rangehat;
}
for (i = 0; i < 2; i++) {
Rangehat = W[i + 2];
Bearinghat = W[i];
Phi_5 = W[i + 4];
Phi_6 = W[i + 6];
for (x_tmp_tmp = 0; x_tmp_tmp <= 0; x_tmp_tmp += 2) {
j = (x_tmp_tmp + 1) << 2;
Q_tmp = x_tmp_tmp << 2;
tmp_6 = (x_tmp_tmp << 1) + i;
tmp_7 = ((x_tmp_tmp + 1) << 1) + i;
_mm_storeu_pd(&tmp_4[0], _mm_add_pd(_mm_add_pd(_mm_add_pd(_mm_add_pd
(_mm_mul_pd(_mm_set_pd(x_tmp[j + 1], x_tmp[Q_tmp + 1]), _mm_set1_pd
(Rangehat)), _mm_mul_pd(_mm_set_pd(x_tmp[j], x_tmp[Q_tmp]),
_mm_set1_pd(Bearinghat))), _mm_mul_pd(_mm_set_pd(x_tmp[j + 2],
x_tmp[Q_tmp + 2]), _mm_set1_pd(Phi_5))), _mm_mul_pd(_mm_set_pd(x_tmp[j +
3], x_tmp[Q_tmp + 3]), _mm_set1_pd(Phi_6))), _mm_set_pd(c_b[tmp_7],
c_b[tmp_6])));
x[tmp_6] = tmp_4[0];
x[tmp_7] = tmp_4[1];
}
}
if (fabs(x[1]) > fabs(x[0])) {
Rangehat = x[0] / x[1];
Bearinghat = 1.0 / (Rangehat * x[3] - x[2]);
b[0] = x[3] / x[1] * Bearinghat;
b[1] = -Bearinghat;
b[2] = -x[2] / x[1] * Bearinghat;
b[3] = Rangehat * Bearinghat;
} else {
Rangehat = x[1] / x[0];
Bearinghat = 1.0 / (x[3] - Rangehat * x[2]);
b[0] = x[3] / x[0] * Bearinghat;
b[1] = -Rangehat * Bearinghat;
b[2] = -x[2] / x[0] * Bearinghat;
b[3] = Bearinghat;
}
for (i = 0; i < 2; i++) {
Rangehat = 0.0;
Bearinghat = 0.0;
Phi_5 = 0.0;
Phi_6 = 0.0;
for (x_tmp_tmp = 0; x_tmp_tmp < 4; x_tmp_tmp++) {
j = x_tmp_tmp << 2;
Q_tmp = (i << 2) + x_tmp_tmp;
tmp_5 = _mm_set1_pd(x_tmp[Q_tmp]);
tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&RadarTracker_DW.P[j]), tmp_5),
_mm_set_pd(Bearinghat, Rangehat));
_mm_storeu_pd(&tmp_4[0], tmp_0);
Rangehat = tmp_4[0];
Bearinghat = tmp_4[1];
tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&RadarTracker_DW.P[j + 2]),
tmp_5), _mm_set_pd(Phi_6, Phi_5));
_mm_storeu_pd(&tmp_4[0], tmp_5);
Phi_5 = tmp_4[0];
Phi_6 = tmp_4[1];
W[Q_tmp] = 0.0;
}
x_tmp_tmp = i << 2;
tmp[x_tmp_tmp + 3] = Phi_6;
tmp[x_tmp_tmp + 2] = Phi_5;
tmp[x_tmp_tmp + 1] = Bearinghat;
tmp[x_tmp_tmp] = Rangehat;
}
for (i = 0; i < 2; i++) {
Q_tmp = i << 2;
Rangehat = W[Q_tmp];
Bearinghat = W[Q_tmp + 1];
Phi_5 = W[Q_tmp + 2];
Phi_6 = W[Q_tmp + 3];
for (x_tmp_tmp = 0; x_tmp_tmp < 2; x_tmp_tmp++) {
j = x_tmp_tmp << 2;
tmp_5 = _mm_set1_pd(b[(i << 1) + x_tmp_tmp]);
tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&tmp[j]), tmp_5), _mm_set_pd
(Bearinghat, Rangehat));
_mm_storeu_pd(&tmp_4[0], tmp_0);
Rangehat = tmp_4[0];
Bearinghat = tmp_4[1];
tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&tmp[j + 2]), tmp_5),
_mm_set_pd(Phi_6, Phi_5));
_mm_storeu_pd(&tmp_4[0], tmp_5);
Phi_5 = tmp_4[0];
Phi_6 = tmp_4[1];
}
W[Q_tmp + 3] = Phi_6;
W[Q_tmp + 2] = Phi_5;
W[Q_tmp + 1] = Bearinghat;
W[Q_tmp] = Rangehat;
}
Rangehat = RadarTracker_Y.residual[1];
Bearinghat = RadarTracker_Y.residual[0];
for (i = 0; i <= 2; i += 2) {
tmp_5 = _mm_loadu_pd(&W[i + 4]);
tmp_0 = _mm_loadu_pd(&W[i]);
tmp_1 = _mm_loadu_pd(&RadarTracker_DW.xhat[i]);
_mm_storeu_pd(&RadarTracker_DW.xhat[i], _mm_add_pd(_mm_add_pd(_mm_mul_pd
(tmp_5, _mm_set1_pd(Rangehat)), _mm_mul_pd(tmp_0, _mm_set1_pd(Bearinghat))),
tmp_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_tmp = j << 2;
Q[j + Q_tmp] = 1.0;
Rangehat = 0.0;
Bearinghat = 0.0;
Phi_5 = 0.0;
Phi_6 = 0.0;
for (i = 0; i < 2; i++) {
x_tmp_tmp = i << 2;
tmp_5 = _mm_set1_pd(M[(j << 1) + i]);
tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&W[x_tmp_tmp]), tmp_5),
_mm_set_pd(Bearinghat, Rangehat));
_mm_storeu_pd(&tmp_4[0], tmp_0);
Rangehat = tmp_4[0];
Bearinghat = tmp_4[1];
tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&W[x_tmp_tmp + 2]), tmp_5),
_mm_set_pd(Phi_6, Phi_5));
_mm_storeu_pd(&tmp_4[0], tmp_5);
Phi_5 = tmp_4[0];
Phi_6 = tmp_4[1];
}
P_tmp[Q_tmp + 3] = Phi_6;
P_tmp[Q_tmp + 2] = Phi_5;
P_tmp[Q_tmp + 1] = Bearinghat;
P_tmp[Q_tmp] = Rangehat;
}
for (i = 0; i <= 14; i += 2) {
tmp_5 = _mm_loadu_pd(&Q[i]);
tmp_0 = _mm_loadu_pd(&P_tmp[i]);
_mm_storeu_pd(&Q_0[i], _mm_sub_pd(tmp_5, tmp_0));
}
for (i = 0; i < 4; i++) {
Rangehat = 0.0;
Bearinghat = 0.0;
Phi_5 = 0.0;
Phi_6 = 0.0;
for (x_tmp_tmp = 0; x_tmp_tmp < 4; x_tmp_tmp++) {
j = x_tmp_tmp << 2;
Q_tmp = (i << 2) + x_tmp_tmp;
tmp_5 = _mm_set1_pd(RadarTracker_DW.P[Q_tmp]);
tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&Q_0[j]), tmp_5), _mm_set_pd
(Bearinghat, Rangehat));
_mm_storeu_pd(&tmp_4[0], tmp_0);
Rangehat = tmp_4[0];
Bearinghat = tmp_4[1];
tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&Q_0[j + 2]), tmp_5),
_mm_set_pd(Phi_6, Phi_5));
_mm_storeu_pd(&tmp_4[0], tmp_5);
Phi_5 = tmp_4[0];
Phi_6 = tmp_4[1];
j += i;
Phi_0[Q_tmp] = (real_T)Phi[j] - P_tmp[j];
}
Q_tmp = i << 2;
Q[Q_tmp + 3] = Phi_6;
Q[Q_tmp + 2] = Phi_5;
Q[Q_tmp + 1] = Bearinghat;
Q[Q_tmp] = Rangehat;
}
for (i = 0; i < 2; i++) {
Rangehat = 0.0;
Bearinghat = 0.0;
Phi_5 = 0.0;
Phi_6 = 0.0;
for (x_tmp_tmp = 0; x_tmp_tmp < 2; x_tmp_tmp++) {
j = x_tmp_tmp << 2;
tmp_5 = _mm_set1_pd(c_b[(i << 1) + x_tmp_tmp]);
tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&W[j]), tmp_5), _mm_set_pd
(Bearinghat, Rangehat));
_mm_storeu_pd(&tmp_4[0], tmp_0);
Rangehat = tmp_4[0];
Bearinghat = tmp_4[1];
tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&W[j + 2]), tmp_5), _mm_set_pd
(Phi_6, Phi_5));
_mm_storeu_pd(&tmp_4[0], tmp_5);
Phi_5 = tmp_4[0];
Phi_6 = tmp_4[1];
}
Q_tmp = i << 2;
M[Q_tmp + 3] = Phi_6;
M[Q_tmp + 2] = Phi_5;
M[Q_tmp + 1] = Bearinghat;
M[Q_tmp] = Rangehat;
}
for (i = 0; i < 4; i++) {
Rangehat = 0.0;
Bearinghat = 0.0;
Phi_5 = 0.0;
Phi_6 = 0.0;
for (x_tmp_tmp = 0; x_tmp_tmp < 4; x_tmp_tmp++) {
j = x_tmp_tmp << 2;
Q_tmp = (i << 2) + x_tmp_tmp;
tmp_5 = _mm_set1_pd(Phi_0[Q_tmp]);
tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&Q[j]), tmp_5), _mm_set_pd
(Bearinghat, Rangehat));
_mm_storeu_pd(&tmp_4[0], tmp_0);
Rangehat = tmp_4[0];
Bearinghat = tmp_4[1];
tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&Q[j + 2]), tmp_5), _mm_set_pd
(Phi_6, Phi_5));
_mm_storeu_pd(&tmp_4[0], tmp_5);
Phi_5 = tmp_4[0];
Phi_6 = tmp_4[1];
P_tmp[Q_tmp] = 0.0;
}
Q_tmp = i << 2;
Q_0[Q_tmp + 3] = Phi_6;
Q_0[Q_tmp + 2] = Phi_5;
Q_0[Q_tmp + 1] = Bearinghat;
Q_0[Q_tmp] = Rangehat;
Rangehat = P_tmp[Q_tmp];
Bearinghat = P_tmp[Q_tmp + 1];
Phi_5 = P_tmp[Q_tmp + 2];
Phi_6 = P_tmp[Q_tmp + 3];
for (x_tmp_tmp = 0; x_tmp_tmp < 2; x_tmp_tmp++) {
j = x_tmp_tmp << 2;
tmp_5 = _mm_set1_pd(W[j + i]);
tmp_0 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&M[j]), tmp_5), _mm_set_pd
(Bearinghat, Rangehat));
_mm_storeu_pd(&tmp_4[0], tmp_0);
Rangehat = tmp_4[0];
Bearinghat = tmp_4[1];
tmp_5 = _mm_add_pd(_mm_mul_pd(_mm_loadu_pd(&M[j + 2]), tmp_5), _mm_set_pd
(Phi_6, Phi_5));
_mm_storeu_pd(&tmp_4[0], tmp_5);
Phi_5 = tmp_4[0];
Phi_6 = tmp_4[1];
}
P_tmp[Q_tmp + 3] = Phi_6;
P_tmp[Q_tmp + 2] = Phi_5;
P_tmp[Q_tmp + 1] = Bearinghat;
P_tmp[Q_tmp] = Rangehat;
}
for (i = 0; i <= 14; i += 2) {
tmp_5 = _mm_loadu_pd(&Q_0[i]);
tmp_0 = _mm_loadu_pd(&P_tmp[i]);
_mm_storeu_pd(&RadarTracker_DW.P[i], _mm_add_pd(tmp_5, tmp_0));
}
/* 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'))
