# 使用 MATLAB Function 模块的雷达跟踪

### 建立参数并初始化加速度数据

• `g` - 重力加速度

• `tauc` - 飞机横轴加速度的相关时间

• `taut` - 飞机推力轴加速度的相关时间

• `speed` - 飞机在 y 方向上的初始速度

• `deltat` - 雷达更新速率

`XY Acceleration Model` 子系统建模并输出加速度数据。Band-Limited White Noise 模块 `INS Acceleration Data` 生成数据，模型使用这些数据来确定飞机在 X-Y 平面的笛卡尔坐标中的加速度数据。

### 查看扩展卡尔曼滤波器

• `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; ```

### 辅助函数

`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)