Main Content

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 블록은 가속도 데이터를 두 번 적분합니다. 이 위치 데이터는 카테시안 좌표이기 때문에 XY to Range Bearing 서브시스템은 위치 데이터를 극좌표로 변환합니다. 실제 레이다 데이터를 더 잘 표현하기 위해 모델은 Band-Limited White Noise 블록을 사용하여 잡음을 생성하고 Gain 블록을 사용해 잡음 강도를 조정하여 위치 데이터에 잡음을 추가합니다. 마지막으로 모델은 Zero-Order Hold 블록인 Sample and Hold를 사용하여 고정 시간 간격으로 연속시간 데이터에 대해 샘플 앤 홀드를 수행한 다음 이를 MATLAB Function 블록의 확장 칼만 필터로 전달합니다.

확장 칼만 필터 확인하기

MATLAB Function 블록을 열어 확장 칼만 필터를 확인합니다. 이 함수는 measured와 deltat라는 두 개의 입력 인수를 받습니다. measured는 극좌표의 입력 위치 데이터이고, deltat는 작업 공간 변수의 값입니다. MATLAB Function 블록 파라미터 변수 구성하기 항목을 참조하십시오. 필터를 구현하기 위해, 함수는 시간 스텝 사이에 저장하는 두 개의 영속 변수 P와 xhat를 정의합니다. 필터를 구현한 후 블록은 다음과 같은 두 개의 출력을 생성합니다.

  • 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

참고 항목

| (System Identification Toolbox)

관련 항목