Main Content

Rastreo de radar con bloques MATLAB Function

Abrir modelo

En este ejemplo se muestra cómo crear un filtro de Kalman para estimar la posición de una aeronave utilizando un bloque MATLAB Function. Después de estimar la posición, el modelo llama a una función externa de MATLAB® para representar los datos de rastreo.

Inspeccionar el modelo

Abra el modelo RadarTrackingExample.

Establecer parámetros e inicializar los datos de aceleración

Para representar el sistema físico, el modelo inicializa estos parámetros en el área de trabajo del modelo:

  • g: aceleración debida a la gravedad

  • tauc: tiempo de correlación de la aceleración del eje transversal de la aeronave

  • taut: tiempo de correlación de la aceleración del eje de propulsión de la aeronave

  • speed: velocidad inicial de la aeronave en la dirección y

  • deltat: frecuencia de actualización del radar

El subsistema XY Acceleration Model modela y produce datos de aceleración como salida. El bloque Band-Limited White Noise, INS Acceleration Data, genera datos que el modelo utiliza para determinar los datos de aceleración de la aeronave en coordenadas cartesianas en el plano X-Y.

Transformar la aceleración en posición

El filtro de Kalman extendido utiliza datos de posición en coordenadas polares. Para obtener la posición de la aeronave, un bloque Second-Order Integrator integra dos veces los datos de aceleración. Como estos datos de posición están expresados en coordenadas cartesianas, el subsistema XY to Range Bearing los convierte a coordenadas polares. Para una mejor representación de los datos reales del radar, el modelo añade ruido a los datos de posición mediante un bloque Band-Limited White Noise y utiliza un bloque Gain para ajustar la intensidad del ruido. Finalmente, el modelo utiliza un bloque Zero-Order Hold, Sample and Hold, para muestrear y retener los datos de tiempo continuo en un intervalo de tiempo fijo antes de pasarlos al filtro de Kalman extendido en el bloque MATLAB Function.

Ver el filtro de Kalman extendido

Abra el bloque MATLAB Function para ver el filtro de Kalman extendido. La función toma dos argumentos de entrada, measured y deltat. measured corresponde a los datos de posición de entrada en coordenadas polares, mientras que deltat corresponde al valor de la variable del área de trabajo. Consulte Configure MATLAB Function Block Parameter Variables. Para implementar el filtro, la función define dos variables persistentes, P y xhat, que almacena entre unidades de tiempo. Después de implementar el filtro, el bloque genera dos salidas:

  • residual: un escalar que contiene el residuo

  • xhatout: un vector que contiene la ubicación y la velocidad estimadas de la aeronave en coordenadas cartesianas

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;

Simular el modelo

Simule el modelo para ver los resultados. El modelo registra las posiciones estimada y actual, y las guarda en el área de trabajo base. A continuación, el modelo utiliza estos datos para representar los resultados al final de la simulación llamando a la función helper plotRadar en el callback StopFcn. En la gráfica se muestran las trayectorias real y estimada en coordenadas polares, el residuo de estimación para el alcance en pies y las posiciones real, medida y estimada.

Función helper

La función plotRadar representa las salidas de datos registrados del bloque 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

Consulte también

| (System Identification Toolbox)

Temas relacionados