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)

도움말 항목