# Radar Tracking Using MATLAB Function Blocks

This example shows how to create a Kalman filter that estimates the position of an aircraft by using a MATLAB Function block. After estimating the position, the model calls an external MATLAB® function to plot the tracking data.

### Inspect the Model

Open the `RadarTrackingExample` model.

### Establish Parameters and Initialize Acceleration Data

To represent the physical system, the model initializes these parameters in the model workspace:

• `g` — Acceleration due to gravity

• `tauc` — Correlation time of aircraft cross axis acceleration

• `taut` — Correlation time of aircraft thrust axis acceleration

• `speed` — Initial speed of aircraft in the y direction

• `deltat` — Radar update rate

The `XY Acceleration Model` subsystem models and outputs the acceleration data. The Band-Limited White Noise block, `INS Acceleration Data`, generates data that the model uses to determine the acceleration data for the aircraft in Cartesian coordinates in the X-Y plane.

### Transform Acceleration to Position

The extended Kalman filter uses position data in polar coordinates. To get the aircraft position, a Second-Order Integrator block integrates the acceleration data twice. Because this position data is in Cartesian coordinates, the `XY to Range Bearing` subsystem converts the position data to polar coordinates. To better represent real radar data, the model adds noise to the position data by using a Band-Limited White Noise block to generate the noise, and a Gain block to adjust the noise intensity. Finally, the model uses a Zero-Order Hold block, `Sample and Hold`, to sample and hold the continuous time data at a fixed-time interval before passing it to the extended Kalman filter in the MATLAB Function block.

### View the Extended Kalman Filter

Open the MATLAB Function block to view the extended Kalman filter. The function takes two input arguments, `measured` and `deltat`. `measured` is the input position data in polar coordinates, and `deltat` is the value of the workspace variable. See Configure MATLAB Function Block Parameter Variables. To implement the filter, the function defines two persistent variables, `P` and `xhat`, that the function stores between time steps. After implementing the filter, the block generates two outputs:

• `residual` — A scalar that contains the residual

• `xhatout` — A vector that contains the aircraft estimated location and velocity in Cartesian coordinates

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

### Simulate the Model

Simulate the model to view the results. The model logs the estimated and actual positions and saves them to the base workspace. The model then uses this data to plot the results at the end of the simulation by calling the helper function, `plotRadar`, in the StopFcn callback. The plot displays the actual and estimated trajectories in polar coordinates, the estimation residual for range in feet, and the actual, measured, and estimated positions.

### Helper Function

The `plotRadar` function plots the logged data outputs from the MATLAB Function block.

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