Rastreo de radar con bloques MATLAB Function
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 gravedadtauc
: tiempo de correlación de la aceleración del eje transversal de la aeronavetaut
: tiempo de correlación de la aceleración del eje de propulsión de la aeronavespeed
: velocidad inicial de la aeronave en la dirección ydeltat
: 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 residuoxhatout
: 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
MATLAB Function | Extended Kalman Filter (System Identification Toolbox)