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)

相关主题