MATLAB Function ブロックを使用したレーダー追跡

この例では、MATLAB Function ブロックを使用して航空機の位置を推定するカルマンフィルターを作成する方法を示します。位置を推定した後、モデルは外部 MATLAB® 関数を呼び出して追跡データをプロットします。

モデルの検査

RadarTrackingExample モデルを開きます。

パラメーターの設定と加速度データの初期化

物理システムを表すために、モデルはモデルワークスペースで次のパラメーターを初期化します。

g — 重力による加速度
tauc — 航空機の交差軸加速度の相関時間
taut — 航空機の推力軸加速度の相関時間
speed — 航空機の y 方向の初速度
deltat — レーダー更新頻度

XY Acceleration Model サブシステムは加速度データをモデル化して出力します。Band-Limited White Noise ブロック INS Acceleration Data は、モデルが "X-Y" 平面の直交座標で航空機の加速度データを決定するために使用するデータを生成します。

加速度の位置への変換

拡張カルマンフィルターは、極座標の位置データを使用します。航空機の位置を取得するために、Second-Order Integratorブロックは加速度データを 2 回積分します。この位置データは直交座標であるため、XY to Range Bearing サブシステムは位置データを極座標に変換します。実際のレーダーデータをより適切に表現するために、モデルは、Band-Limited White Noise ブロックを使用してノイズを生成し、Gain ブロックを使用してノイズ強度を調整することにより、位置データにノイズを追加します。最後に、モデルは Zero-Order Hold ブロック Sample and Hold を使用して、固定時間間隔で連続時間データをサンプリングして保持してから、MATLAB Function ブロックの拡張カルマンフィルターに渡します。

拡張カルマンフィルターの表示

MATLAB Function ブロックを開いて、拡張カルマンフィルターを表示します。関数は 2 つの入力引数 measured と deltat を受け取ります。measured は極座標での入力位置データ、deltat はワークスペース変数の値です。MATLAB Function ブロックパラメーター変数の設定を参照してください。フィルターを実装するために、関数は 2 つの永続変数 P と xhat を定義し、タイムステップ間に関数でこれらの変数を保存します。フィルターを実装した後、ブロックは次の 2 つの出力を生成します。

residual — 残差を含むスカラー
xhatout — 直交座標での航空機の推定位置と速度を含むベクトル

function [residual, xhatOut] = extendedKalman(measured, deltat)
% Radar Data Processing Tracker Using an Extended Kalman Filter

%% Initialization
persistent P;
persistent xhat
if isempty(P)
    xhat = [0.001; 0.01; 0.001; 400];
    P = zeros(4);
end

%% Compute Phi, Q, and R
Phi = [1 deltat 0 0; 0 1 0 0 ; 0 0 1 deltat; 0 0 0 1];
Q =  diag([0 .005 0 .005]);
R =  diag([300^2 0.001^2]);

%% Propagate the covariance matrix and track estimate
P = Phi*P*Phi' + Q;
xhat = Phi*xhat;

%% Compute observation estimates:
Rangehat = sqrt(xhat(1)^2+xhat(3)^2);
Bearinghat = atan2(xhat(3),xhat(1));

% Compute observation vector y and linearized measurement matrix M
yhat = 	[Rangehat;
            Bearinghat];
M = [ cos(Bearinghat)          0 sin(Bearinghat)          0
    -sin(Bearinghat)/Rangehat 0 cos(Bearinghat)/Rangehat 0 ];

%% Compute residual (Estimation Error)
residual = measured - yhat;

% Compute Kalman Gain:
W = P*M'/(M*P*M'+ R);

% Update estimate
xhat = xhat + W*residual;

% Update Covariance Matrix
P = (eye(4)-W*M)*P*(eye(4)-W*M)' + W*R*W';

xhatOut = xhat;

モデルのシミュレーション

モデルをシミュレートして結果を表示します。モデルは推定位置と実際の位置をログに記録し、ベースワークスペースに保存します。次に、モデルは "StopFcn" コールバックで補助関数 plotRadar を呼び出すことにより、シミュレーションの終了時にこのデータを使用して結果をプロットします。プロットには、極座標での実際の軌跡と推定の軌跡、フィート単位での距離の推定残差、実際の位置、測定位置、および推定位置が表示されます。

補助関数

関数 plotRadar は、MATLAB Function ブロックからのログデータ出力をプロットします。

function plotRadar(varargin)
% Radar Data Processing Tracker plotting function

% Get radar measurement interval from model
deltat = 1;

% Get logged data from workspace
data = locGetData();

if isempty(data)
    return;  % if there is no data, no point in plotting
else
    XYCoords          = data.XYCoords;
    measurementNoise  = data.measurementNoise;
    polarCoords       = data.polarCoords;
    residual          = data.residual;
    xhat              = data.xhat;
end

% Plot data: set up figure
if nargin > 0
    figTag = varargin{1};
else
    figTag = 'no_arg';
end

figH = findobj('Type','figure','Tag', figTag);

if isempty(figH)
    figH = figure;
    set(figH,'WindowState','maximized','Tag',figTag);
end

clf(figH)

% Polar plot of actual/estimated position
figure(figH); % keep focus on figH
axesH = subplot(2,3,1,polaraxes);
polarplot(axesH,polarCoords(:,2) - measurementNoise(:,2), ...
        polarCoords(:,1) - measurementNoise(:,1),'r')

hold on
rangehat = sqrt(xhat(:,1).^2+xhat(:,3).^2);
bearinghat = atan2(xhat(:,3),xhat(:,1));
polarplot(bearinghat,rangehat,'g');
legend(axesH,'Actual','Estimated','Location','south');

% Range Estimate Error
figure(figH); % keep focus on figH
axesH = subplot(2,3,4);
plot(axesH, residual(:,1)); grid; set(axesH,'xlim',[0 length(residual)]);
xlabel(axesH, 'Number of Measurements');
ylabel(axesH, 'Range Estimate Error - Feet')
title(axesH, 'Estimation Residual for Range')

% East-West position
XYMeas    = [polarCoords(:,1).*cos(polarCoords(:,2)), ...
            polarCoords(:,1).*sin(polarCoords(:,2))];
numTSteps = size(XYCoords,1);
t_full    = 0.1 * (0:numTSteps-1)';
t_hat     = (0:deltat:t_full(end))';

figure(figH); % keep focus on figH
axesH = subplot(2,3,2:3);
plot(axesH, t_full,XYCoords(:,2),'r');
grid on;hold on
plot(axesH, t_full,XYMeas(:,2),'g');
plot(axesH, t_hat,xhat(:,3),'b');
title(axesH, 'E-W Position');
legend(axesH, 'Actual','Measured','Estimated','Location','Northwest');
hold off

% North-South position
figure(figH); % keep focus on figH
axesH = subplot(2,3,5:6);
plot(axesH, t_full,XYCoords(:,1),'r');
grid on;hold on
plot(axesH, t_full,XYMeas(:,1),'g');
plot(axesH, t_hat,xhat(:,1),'b');
xlabel(axesH, 'Time (sec)');
title(axesH, 'N-S Position');
legend(axesH, 'Actual','Measured','Estimated','Location','Northwest');
hold off
end

% Function "locGetData" logs data to workspace
function data = locGetData
% Get simulation result data from workspace

% If necessary, convert logged signal data to local variables
if evalin('base','exist(''radarLogsOut'')')
    try
        logsOut = evalin('base','radarLogsOut');
        if isa(logsOut, 'Simulink.SimulationData.Dataset')
            data.measurementNoise = logsOut.get('measurementNoise').Values.Data;
            data.XYCoords         = logsOut.get('XYCoords').Values.Data;
            data.polarCoords      = logsOut.get('polarCoords').Values.Data;
            data.residual         = logsOut.get('residual').Values.Data;
            data.xhat             = logsOut.get('xhat').Values.Data;
        else
            assert(isa(logsOut, 'Simulink.ModelDataLogs'));
            data.measurementNoise = logsOut.measurementNoise.Data;
            data.XYCoords         = logsOut.XYCoords.Data;
            data.polarCoords      = logsOut.polarCoords.Data;
            data.residual         = logsOut.residual.Data;
            data.xhat             = logsOut.xhat.Data;
        end
    catch %#ok<CTCH>
        data = [];
    end
else
    if evalin('base','exist(''measurementNoise'')')
        data.measurementNoise  = evalin('base','measurementNoise');
        data.XYCoords          = evalin('base','XYCoords');
        data.polarCoords       = evalin('base','polarCoords');
        data.residual          = evalin('base','residual');
        data.xhat              = evalin('base','xhat');
    else
        data = [];  % something didn't run, skip retrieval
    end
end
end

参考

MATLAB Function | Extended Kalman Filter (System Identification Toolbox)