## Track Object Using MATLAB Code

### Learning Objectives

In this tutorial, you will learn how to:

• Use the MATLAB Function block to add MATLAB® functions to Simulink® models for modeling, simulation, and deployment to embedded processors.

This capability is useful for coding algorithms that are better stated in the textual language of MATLAB than in the graphical language of Simulink.

• Use coder.extrinsic to call MATLAB code from a MATLAB Function block.

This capability allows you to do rapid prototyping. You can call existing MATLAB code from Simulink without having to make this code suitable for code generation.

• Check that existing MATLAB code is suitable for code generation before generating code.

You must prepare your code before generating code.

• Specify variable-size inputs when generating code.

### Tutorial Prerequisites

#### What You Need to Know

To complete this tutorial, you should have basic familiarity with MATLAB software. You should also understand how to create and simulate a basic Simulink model.

#### Required Products

To complete this tutorial, you must install the following products:

• MATLAB

• MATLAB Coder™

• C compiler

For a list of supported compilers, see Supported Compilers.

You must set up the C compiler before generating C code. See Setting Up Your C Compiler.

For instructions on installing MathWorks® products, see the MATLAB installation documentation for your platform. If you have installed MATLAB and want to check which other MathWorks products are installed, enter ver in the MATLAB Command Window.

### Example: The Kalman Filter

#### Description

This section describes the example used by the tutorial. You do not have to be familiar with the algorithm to complete the tutorial.

The example for this tutorial uses a Kalman filter to estimate the position of an object moving in a two-dimensional space from a series of noisy inputs based on past positions. The position vector has two components, x and y, indicating its horizontal and vertical coordinates.

Kalman filters have a wide range of applications, including control, signal and image processing; radar and sonar; and financial modeling. They are recursive filters that estimate the state of a linear dynamic system from a series of incomplete or noisy measurements. The Kalman filter algorithm relies on the state-space representation of filters and uses a set of variables stored in the state vector to characterize completely the behavior of the system. It updates the state vector linearly and recursively using a state transition matrix and a process noise estimate.

#### Algorithm

This section describes the algorithm of the Kalman filter and is implemented in the MATLAB version of the filter supplied with this tutorial.

The algorithm predicts the position of a moving object based on its past positions using a Kalman filter estimator. It estimates the present position by updating the Kalman state vector, which includes the position (x and y), velocity (Vx and Vy), and acceleration (Ax and Ay) of the moving object. The Kalman state vector, x_est, is a persistent variable.

% Initial conditions
persistent x_est p_est
if isempty(x_est)
x_est = zeros(6, 1);
p_est = zeros(6, 6);
end
x_est is initialized to an empty 6x1 column vector and updated each time the filter is used.

The Kalman filter uses the laws of motion to estimate the new state:

$\begin{array}{l}X={X}_{0}+Vx.dt\\ Y={Y}_{0}+Vy.dt\\ Vx=V{x}_{0}+Ax.dt\\ Vy=V{y}_{0}+Ay.dt\end{array}$

These laws of motion are captured in the state transition matrix A, which is a matrix that contains the coefficient values of x, y, Vx, Vy, Ax, and Ay.

% Initialize state transition matrix
dt=1;
A=[ 1 0 dt 0 0 0;...
0 1 0 dt 0 0;...
0 0 1 0 dt 0;...
0 0 0 1 0 dt;...
0 0 0 0 1 0 ;...
0 0 0 0 0 1 ];

#### Filtering Process

The filtering process has two phases:

• Predicted state and covariance

The Kalman filter uses the previously estimated state, x_est, to predict the current state, x_prd. The predicted state and covariance are calculated in:

% Predicted state and covariance
x_prd = A * x_est;
p_prd = A * p_est * A' + Q;

• Estimation

The filter also uses the current measurement, z, and the predicted state, x_prd, to estimate a more accurate approximation of the current state. The estimated state and covariance are calculated in:

% Measurement matrix
H = [ 1 0 0 0 0 0; 0 1 0 0 0 0 ];
Q = eye(6);
R = 1000 * eye(2);
% Estimation
S = H * p_prd' * H' + R;
B = H * p_prd';
klm_gain = (S \ B)';

% Estimated state and covariance
x_est = x_prd + klm_gain * (z - H * x_prd);
p_est = p_prd - klm_gain * H * p_prd;

% Compute the estimated measurements
y = H * x_est;

#### Reference

Haykin, Simon. Adaptive Filter Theory. Upper Saddle River, NJ: Prentice-Hall, Inc., 1996.

### Files for the Tutorial

The tutorial uses the following files:

• Simulink model files for each step of the tutorial.

• Example MATLAB code files for each step of the tutorial.

Throughout this tutorial, you work with Simulink models that call MATLAB files containing a Kalman filter algorithm.

• A MAT-file that contains example input data.

• A MATLAB file for plotting.

#### Location of Files

The tutorial files are available in the following folder: docroot\toolbox\simulink\examples\kalman. To run the tutorial, you must copy these files to a local folder. For instructions, see Copying Files Locally.

#### Names and Descriptions of Files

TypeNameDescription
MATLAB function filesex_kalman01Baseline MATLAB implementation of a scalar Kalman filter.
ex_kalman02Version of the original algorithm suitable for code generation.
ex_kalman03Version of Kalman filter suitable for code generation and for use with frame-based and packet-based inputs.
ex_kalman04Disabled inlining for code generation.
ex_kalman11Complete Simulink model with a MATLAB Function block for scalar Kalman filter.
ex_kalman22Simulink model with a MATLAB Function block for a Kalman filter that accepts fixed-size (frame-based) inputs.
ex_kalman33Simulink model with a MATLAB Function block for a Kalman filter that accepts variable-size (packet-based) inputs.
ex_kalman44Simulink model to call ex_kalman04.m, which has inlining disabled.
MATLAB data filepositionContains the input data used by the algorithm.
Plot filesplot_trajectoryPlots the trajectory of the object and the Kalman filter estimated position.

### Tutorial Steps

#### Copying Files Locally

Copy the tutorial files to a local working folder:

1. Create a local solutions folder, for example, c:\simulink\kalman\solutions.

2. Change to the docroot\toolbox\simulink\examples folder. At the MATLAB command line, enter:

3. Copy the contents of the kalman subfolder to your local solutions folder, specifying the full path name of the solutions folder:

copyfile('kalman', 'solutions')

For example:

Your solutions folder now contains a complete set of solutions for the tutorial. If you do not want to perform the steps for each task in the tutorial, you can view the solutions to see how the code should look.

4. Create a local work folder, for example, c:\simulink\kalman\work.

5. Copy the following files from your solutions folder to your work folder.

• ex_kalman01

• ex_kalman00

• position

• plot_trajectory

Your work folder now contains all the files that you need to get started with the tutorial.

#### Setting Up Your C Compiler

Building your MATLAB Function block requires a supported compiler. MATLAB automatically selects one as the default compiler. If you have multiple MATLAB-supported compilers installed on your system, you can change the default using the mex -setup command. See Change Default Compiler (MATLAB).

First, examine the ex_kalman00 model supplied with the tutorial to understand the problem that you are trying to solve using the Kalman filter.

1. Open the ex_kalman00 model in Simulink:

1. Set your MATLAB current folder to the folder that contains your working files for this tutorial. At the MATLAB command line, enter:

cd work
where work is the full path name of the folder containing your files.

2. At the MATLAB command line, enter:

ex_kalman00

This model is an incomplete model to demonstrate how to integrate MATLAB code with Simulink. The complete model is ex_kalman11, which is also supplied with this tutorial.

InitFcn Model Callback Function.  The model uses this callback function to:

• Load position data from a MAT-file.

• Set up data used by the Index generator block, which provides the second input to the Selector block.

To view this callback:

1. On the Modeling tab, select Model Settings > Model Properties.

2. Select the Callbacks tab.

3. Select InitFcn in the Model callbacks pane.

The callback appears.

[R,C]=size(position);
idx=(1:C)';
t=idx-1;

Source Blocks.  The model uses two Source blocks to provide position data and a scalar index to a Selector block.

Selector Block.  The model uses a Selector block that selects elements of its input signal and generates an output signal based on its index input and its Index Option settings. By changing the configuration of this block, you can generate different size signals.

To view the Selector block settings, double-click the Selector block to view the function block parameters.

In this model, the Index Option for the first port is Select all and for the second port is Index vector (port). Because the input is a 2 x 310 position matrix, and the index data increments from 1 to 310, the Selector block simply outputs one 2x1 output at each sample time.

MATLAB Function Block.  The model uses a MATLAB Function block to plot the trajectory of the object and the Kalman filter estimated position. This function:

• First declares the figure, hold, and plot_trajectory functions as extrinsic because these MATLAB visualization functions are not supported for code generation. When you call an unsupported MATLAB function, you must declare it to be extrinsic so MATLAB can execute it, but does not try to generate code for it.

• Creates a figure window and holds it for the duration of the simulation. Otherwise a new figure window appears for each sample time.

• Calls the plot_trajectory function, which plots the trajectory of the object and the Kalman filter estimated position.

Simulation Stop Time.  The simulation stop time is 309, because the input to the filter is a vector containing 310 elements and Simulink uses zero-based indexing.

To modify the model and code yourself, work through the exercises in this section. Otherwise, open the supplied model ex_kalman11 in your solutions subfolder to see the modified model.

For the purposes of this tutorial, you add the MATLAB Function block to the ex_kalman00.mdl model supplied with the tutorial. You would have to develop your own test bench starting with an empty Simulink model.

Adding the MATLAB Function Block.  To add a MATLAB Function block to the ex_kalman00 model:

ex_kalman00

2. Add a MATLAB Function block to the model:

1. At the MATLAB command line, type slLibraryBrowser to open the Simulink Library Browser.

2. From the list of Simulink libraries, select the User-Defined Functions library.

3. Click the MATLAB Function block and drag it into the ex_kalman00 model. Place the block just above the red text annotation that reads Place MATLAB Function Block here.

4. Delete the red text annotations from the model.

5. Save the model in the current folder as ex_kalman11.

Calling Your MATLAB Code from the MATLAB Function Block.   To call your MATLAB code from the MATLAB Function block:

1. Double-click the MATLAB Function block to open the MATLAB Function Block Editor.

2. Delete the default code displayed in the editor.

3. Copy the following code to the MATLAB Function block.

function y = kalman(u)
%#codegen

y = ex_kalman01(u);

4. Save the model.

Connecting the MATLAB Function Block Input and Output

1. Connect the MATLAB Function block input and output so that your model looks like this.

2. Save the model.

#### Simulating the ex_kalman11 Model

To simulate the model:

1. In the Simulink model window, click Run.

As Simulink runs the model, it plots the trajectory of the object in blue and the Kalman filter estimated position in green. Initially, you see that it takes a short time for the estimated position to converge with the actual position of the object. Then three sudden shifts in position occur—each time the Kalman filter readjusts and tracks the object after a few iterations.

2. The simulation stops.

You have proved that your MATLAB algorithm works in Simulink. You are now ready to modify the filter to accept a fixed-size input, as described in Modifying the Filter to Accept a Fixed-Size Input.

#### Modifying the Filter to Accept a Fixed-Size Input

The filter you have worked on so far in this tutorial uses a simple batch process that accepts one input at a time, so you must call the function repeatedly for each input. In this part of the tutorial, you learn how to modify the algorithm to accept a fixed-sized input, which makes the algorithm suitable for frame-based processing. You then modify the model to provide the input as fixed-size frames of data and call the filter passing in the data one frame at a time.

Modifying Your MATLAB Code.  To modify the code yourself, work through the exercises in this section. Otherwise, open the supplied file ex_kalman03.m in your solutions subfolder to see the modified algorithm.

You can now modify the algorithm to process a vector containing more than one input. You need to find the length of the vector and call the filter code for each element in the vector in turn. You do this by calling the filter algorithm in a for loop.

1. Open ex_kalman02.m in the MATLAB Editor. At the MATLAB command line, enter:

edit ex_kalman02.m

2. Add a for loop around the filter code.

1. Before the comment:

% Predicted state and covariance
insert:
for i=1:size(z,2)

2. After:

% Compute the estimated measurements
y = H * x_est;
insert:
end

3. Select the code between the for statement and the end statement, right-click to open the context menu and select Smart Indent to indent the code.

Your filter code should now look like this:

for i=1:size(z,2)
% Predicted state and covariance
x_prd = A * x_est;
p_prd = A * p_est * A' + Q;

% Estimation
S = H * p_prd' * H' + R;
B = H * p_prd';
klm_gain = (S \ B)';

% Estimated state and covariance
x_est = x_prd + klm_gain * (z - H * x_prd);
p_est = p_prd - klm_gain * H * p_prd;

% Compute the estimated measurements
y = H * x_est;
end

3. Modify the line that calculates the estimated state and covariance to use the ith element of input z.

Change:

x_est = x_prd + klm_gain * (z - H * x_prd);
to:
x_est = x_prd + klm_gain * (z(1:2,i) - H * x_prd);

4. Modify the line that computes the estimated measurements to append the result to the ith element of the output y.

Change:

y = H * x_est;
to:
y(:,i) = H * x_est;

The code analyzer message indicator in the top right turns orange to indicate that the code analyzer has detected warnings. The code analyzer underlines the offending code in orange and places a orange marker to the right.

5. Move your pointer over the orange marker to view the error information.

The code analyzer detects that y must be fully defined before sub-scripting it and that you cannot grow variables through indexing in generated code.

6. To address this warning, preallocate memory for the output y, which is the same size as the input z. Add this code before the for loop.

% Pre-allocate output signal:
y=zeros(size(z));

The orange marker disappears and the code analyzer message indicator in the top right edge of the code turns green, which indicates that you have fixed all the errors and warnings detected by the code analyzer.

7. Change the function name to ex_kalman03 and save the file as ex_kalman03.m in the current folder.

You are ready to begin the next task in the tutorial, Modifying Your Model to Call the Updated Algorithm.

Modifying Your Model to Call the Updated Algorithm.  To modify the model yourself, work through the exercises in this section. Otherwise, open the supplied model ex_kalman22.mdl in your solutions subfolder to see the modified model.

Next, update your model to provide the input as fixed-size frames of data and call ex_kalman03 passing in the data one frame at a time.

1. Open ex_kalman11 model in Simulink.

ex_kalman11

2. Double-click the MATLAB Function block to open the MATLAB Function Block Editor.

3. Replace the code that calls ex_kalman02 with a call to ex_kalman03.

function y = kalman(u)
%#codegen

y = ex_kalman03(u);

4. Close the editor.

5. Modify the InitFcn callback:

1. On the Modeling tab, select Model Settings > Model Properties.

The Model Properties dialog box opens.

2. In this dialog box, select the Callbacks tab.

3. Select InitFcn in the Model callbacks pane.

4. Replace the existing callback with:

[R,C]=size(position);
FRAME_SIZE=5;
idx=(1:FRAME_SIZE:C)';
LEN=length(idx);
t=(1:LEN)'-1;

This callback sets the frame size to 5, and the index to increment by 5.

5. Click and close the Model Properties dialog box.

6. Update the Selector block to use the correct indices.

1. Double-click the Selector block to view the function block parameters.

The Function Block Parameters dialog box opens.

2. Set the second Index Option to Starting index (port).

3. Set the Output Size for the second input to FRAME_SIZE, click and close the dialog box.

Now, the Index Option for the first port is Select all and for the second port is Starting index (port). Because the index increments by 5 each sample time, and the output size is 5, the Selector block outputs a 2x5 output at each sample time.

7. Change the model simulation stop time to 61. Now the frame size is 5, so the simulation completes in a fifth of the sample times.

1. In the Simulink model window, on the Modeling tab, click Model Settings.

2. In the left pane of the Configuration Parameters dialog box, select Solver.

3. In the right pane, set Stop time to 61.

4. Click and close the dialog box.

8. Save the model as ex_kalman22.mdl.

Testing Your Modified Algorithm.  To simulate the model:

1. In the Simulink model window, Click Run.

As Simulink runs the model, it plots the trajectory of the object in blue and the Kalman filter estimated position in green as before when you used the batch filter.

2. The simulation stops.

You have proved that your algorithm accepts a fixed-size signal. You are now ready for the next task, Using the Filter to Accept a Variable-Size Input.

#### Using the Filter to Accept a Variable-Size Input

In this part of the tutorial, you learn how to specify variable-size data in your Simulink model. Then you test your Kalman filter algorithm with variable-size inputs and see that the algorithm is suitable for processing packets of data of varying size. For more information on using variable-size data in Simulink, see Variable-Size Signal Basics.

Updating the Model to Use Variable-Size Inputs.  To modify the model yourself, work through the exercises in this section. Otherwise, open the supplied model ex_kalman33.mdl in your solutions subfolder to see the modified model.

ex_kalman22

2. Modify the InitFcn callback:

1. On the Modeling tab, select Model Settings > Model Properties.

The Model Properties dialog box opens.

2. Select the Callbacks tab.

3. Select InitFcn in the Model callbacks pane.

4. Replace the existing callback with:

idx=[ 1 1 ;2 3 ;4 6 ;7 10 ;11 15 ;16 30 ;
31 70 ;71 100 ;101 200 ;201 250 ;251 310];
LEN=length(idx);
t=(0:1:LEN-1)';
This callback sets up indexing to generate eleven different size inputs. It specifies the start and end indices for each sample time. The first sample time uses only the first element, the second sample time uses the second and third elements, and so on. The largest sample, 101 to 200, contains 100 elements.

5. Click and close the Model Properties dialog box.

3. Update the Selector block to use the correct indices.

1. Double-click the Selector block to view the function block parameters.

The Function Block Parameters dialog box opens.

2. Set the second Index Option to Starting and ending indices (port), then click and close the dialog box.

This setting means that the input to the index port specifies the start and end indices for the input at each sample time. Because the index input specifies different starting and ending indices at each sample time, the Selector block outputs a variable-size signal as the simulation progresses.

4. Use the Ports and Data Manager to set the MATLAB Function input x and output y as variable-size data.

1. Double-click the MATLAB Function block to open the MATLAB Function Block Editor.

2. From the editor menu, select Edit Data.

3. In the Ports and Data Manager left pane, select the input u.

The Ports and Data Manager displays information about u in the right pane.

4. On the General tab, select the Variable size check box and click .

5. In the left pane, select the output y.

6. On the General tab:

1. Set the Size of y to [2 100] to specify a 2-D matrix where the upper bounds are 2 for the first dimension and 100 for the second, which is the maximum size input specified in the InitFcn callback.

2. Select the Variable size check box.

3. Click .

7. Close the Ports and Data Manager.

5. Now do the same for the other MATLAB Function block. Use the Ports and Data Manager to set the Visualizing block inputs y and z as variable-size data.

1. Double-click the Visualizing block to open the MATLAB Function Block Editor.

2. From the editor menu, select Edit Data.

3. In the Ports and Data Manager left pane, select the input y.

4. On the General tab, select the Variable size check box and click .

5. In the left pane, select the input z.

6. On the General tab, select the Variable size check box and click .

7. Close the Ports and Data Manager.

6. Change the model simulation stop time to 10. This time, the filter processes one of the eleven different size inputs each sample time.

7. Save the model as ex_kalman33.mdl.

Testing Your Modified Model.  To simulate the model:

1. In the Simulink model window, click Run.

As Simulink runs the model, it plots the trajectory of the object in blue and the Kalman filter estimated position in green as before.

Note that the signal lines between the Selector block and the Tracking and Visualization blocks change to show that these signals are variable-size.

2. The simulation stops.

You have successfully created an algorithm that accepts variable-size inputs. Next, you learn how to debug your MATLAB Function block, as described in Debugging the MATLAB Function Block.

#### Debugging the MATLAB Function Block

You can debug your MATLAB Function block just like you can debug a function in MATLAB.

1. Double-click the MATLAB Function block that calls the Kalman filter to open the MATLAB Function Block Editor.

2. In the editor, click the dash (-) character in the left margin of the line:

y = kalman03(u);

A small red ball appears in the margin of this line, indicating that you have set a breakpoint.

3. In the Simulink model window, click Run.

The simulation pauses when execution reaches the breakpoint and a small green arrow appears in the left margin.

4. Place the pointer over the variable u.

The value of u appears adjacent to the pointer.

5. From the MATLAB Function Block Editor menu, select Step In.

The kalman03.m file opens in the editor and you can now step through this code using Step, Step In, and Step Out.

6. Select Step Out.

The kalman03.m file closes and the MATLAB Function block code reappears in the editor.

7. Place the pointer over the output variable y.

You can now see the value of y.

8. Click the red ball to remove the breakpoint.

9. From the MATLAB Function Block Editor menu, select Quit Debugging.

10. Close the editor.

11. Close the figure window.

### Note

Before generating code, you must check that your MATLAB code is suitable for code generation. If you call your MATLAB code as an extrinsic function, you must remove extrinsic calls before generating code.

1. Rename the MATLAB Function block to Tracking. To rename the block, double-click the annotation MATLAB Function below the MATLAB Function block and replace the text with Tracking.

When you generate code for the MATLAB Function block, Simulink Coder uses the name of the block in the generated code. It is good practice to use a meaningful name.

2. Before generating code, ensure that Simulink Coder creates a code generation report. This HTML report provides easy access to the list of generated files with a summary of the configuration settings used to generate the code.

1. In the Simulink model window, on the Modeling tab, click Model Settings.

The Configuration Parameters dialog box opens.

2. In the left pane of the Configuration Parameters dialog box, select Report under Code Generation.

3. In the right pane, select Create code generation report.

The Open report automatically option is also selected.

4. Click and close the Configuration Parameters dialog box.

3. To generate code for the Tracking block:

1. Right-click the Tracking block and select C/C++ Code > Build Selected Subsystem.

2. In the Build code for Subsystem window, click Build. For more information, see Generate Code and Executables for Individual Subsystems (Simulink Coder).

4. The Simulink software generates an error informing you that it cannot log variable-size signals as arrays. You need to change the format of data saved to the MATLAB workspace. To change this format:

• In the Simulink model window, on the Modeling tab, click Model Settings.

The Configuration Parameters dialog box opens.

• In the left pane of the Configuration Parameters dialog box, select Data Import/Export and set the Format to Structure with time.

The logged data is now a structure that has two fields: a time field and a signals field, enabling Simulink to log variable-size signals.

• Click and close the Configuration Parameters dialog box.

5. Repeat step 3 to generate code for the Tracking block.

The Simulink Coder software generates C code for the block and launches the code generation report.

6. In the left pane of the code generation report, click the Tracking.c link to view the generated C code. Note that in the code generated for the MATLAB Function block, Tracking, there might be no separate function code for the ex_kalman03 function because function inlining is enabled by default.

7. Modify your filter algorithm to disable inlining:

1. In ex_kalman03.m, after the function declaration, add:

coder.inline('never');

2. Change the function name to ex_kalman04 and save the file as ex_kalman04.m in the current folder.

3. In your ex_kalman33 model, double-click the Tracking block.

The MATLAB Function Block Editor opens.

4. Modify the call to the filter algorithm to call ex_kalman04.

function y = kalman(u)
%#codegen

y = ex_kalman04(u);

5. Save the model as ex_kalman44.mdl.

8. Generate and inspect the C code.

1. Repeat step 3.

2. In the left pane of the code generation report, click the Tracking.c link to view the generated C code.

Inspect the generated C code for the ex_kalman04 function.

 /* Forward declaration for local functions */ static void Tracking_ex_kalman04(const real_T z_data[620], const int32_T z_sizes[2], real_T y_data[620], int32_T y_sizes[2]); /* Function for MATLAB Function Block: '/Tracking' */ static void Tracking_ex_kalman04(const real_T z_data[620], const int32_T 48 z_sizes[2], real_T y_data[620], int32_T y_sizes[2])

### Key Points to Remember

• Back up your MATLAB code before you modify it.

• Decide on a naming convention for your files and save interim versions frequently. For example, this tutorial uses a two-digit suffix to differentiate the various versions of the filter algorithm.

• For simulation purposes, before generating code, call your MATLAB code using coder.extrinsic to check that your algorithm is suitable for use in Simulink. This practice provides these benefits:

• You do not have to make the MATLAB code suitable for code generation.

• You can debug your MATLAB code in MATLAB while calling it from Simulink.

• Create a Simulink Coder code generation report. This HTML report provides easy access to the list of generated files with a summary of the configuration settings used to generate the code.