Main Content

Tracking radar utilizzando i blocchi MATLAB Function

Apri modello

Questo esempio mostra come creare un filtro di Kalman che stima la posizione di un aereo utilizzando un blocco MATLAB Function. Dopo aver stimato la posizione, il modello richiama una funzione MATLAB® esterna per tracciare i dati di tracking.

Ispezione del modello

Aprire il modello RadarTrackingExample.

Definizione dei parametri e inizializzazione dei dati di accelerazione

Per rappresentare il sistema fisico, il modello inizializza questi parametri nel workspace del modello:

  • g: accelerazione dovuta alla gravità

  • tauc: tempo di correlazione dell'accelerazione trasversale dell'aereo

  • taut: tempo di correlazione dell'accelerazione lungo l'asse di spinta dell'aereo

  • speed: velocità iniziale dell'aereo in direzione y

  • deltat: velocità di aggiornamento del radar

Il sottosistema XY Acceleration Model modella e genera i dati di accelerazione. Il blocco Band-Limited White Noise INS Acceleration Data genera data i dati che il modello utilizza per determinare i dati di accelerazione dell'aereo in coordinate cartesiane nel piano X-Y.

Trasformazione dell'accelerazione in posizione

Il filtro di Kalman esteso utilizza dati di posizione in coordinate polari. Per ottenere la posizione dell'aereo, un blocco Second-Order Integrator integra i dati di accelerazione due volte. Poiché i dati di posizione sono in coordinate cartesiane, il sottosistema XY to Range Bearing converte i dati di posizione in coordinate polari. Per rappresentare meglio i dati reali del radar, il modello aggiunge un rumore ai dati di posizione utilizzando un blocco Band-Limited White Noise per generare il rumore e un blocco Gain per regolare l'intensità dello stesso. Infine, il modello utilizza un blocco Zero-Order Hold Sample and Hold, per campionare e mantenere i dati a tempo continuo a un intervallo di tempo fisso prima di passarli al filtro di Kalman esteso nel blocco MATLAB Function.

Visualizzazione del filtro di Kalman esteso

Aprire il blocco MATLAB Function per visualizzare il filtro di Kalman esteso. La funzione accetta due argomenti di input, measured e deltat. measured è il dato della posizione in input in coordinate polari, mentre deltat è il valore della variabile nel workspace. Vedere Configure MATLAB Function Block Parameter Variables. Per implementare il filtro, la funzione definisce due variabili persistenti P e xhat, che la funzione memorizza tra le fasi temporali. Dopo aver implementato il filtro, il blocco genera due output:

  • residual: uno scalare che contiene il residuo

  • xhatout: un vettore che contiene la posizione e la velocità stimata dell'aereo in coordinate cartesiane

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;

Simulazione del modello

Simulare il modello per visualizzare i risultati. Il modello registra le posizioni stimate e reali e le salva nel workspace di base. Il modello utilizza quindi questi dati per tracciare i risultati al termine della simulazione, richiamando la funzione di supporto plotRadar nella chiamata StopFcn. Il grafico visualizza le traiettorie reali e stimate in coordinate polari, il residuo di stima per la portata in piedi e le posizioni reali, misurate e stimate.

Funzione di supporto

La funzione plotRadar traccia i dati registrati in output dal blocco 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

Vedi anche

| (System Identification Toolbox)

Argomenti complementari