C Code Generation for a MATLAB Kalman Filtering Algorithm

This example shows how to generate C code for a MATLAB® Kalman filter function, kalmanfilter, which estimates the position of a moving object based on past noisy measurements. It also shows how to generate a MEX function for this MATLAB code to increase the execution speed of the algorithm in MATLAB.

Prerequisites

There are no prerequisites for this example.

About the kalmanfilter Function

The kalmanfilter function predicts the position of a moving object based on its past values. It uses a Kalman filter estimator, a recursive adaptive filter that estimates the state of a dynamic system from a series of noisy measurements. Kalman filtering has a broad range of application in areas such as signal and image processing, control design, and computational finance.

About the Kalman Filter Estimator Algorithm

The Kalman estimator computes the position vector by computing and updating the Kalman state vector. The state vector is defined as a 6-by-1 column vector that includes position (x and y), velocity (Vx Vy), and acceleration (Ax and Ay) measurements in a 2-dimensional Cartesian space. Based on the classical laws of motion:

$$\{\begin{array}{rcl}X& =& X_0+V_{x}dt\\ Y& =& Y_0+V_{y}dt\\ V_x& =& V_{x0}+A_{x}dt\\ V_y& =& V_{y0}+A_{y}dt\end{array}$$

The iterative formula capturing these laws are reflected in the Kalman state transition matrix "A". Note that by writing about 10 lines of MATLAB code, you can implement the Kalman estimator based on the theoretical mathematical formula found in many adaptive filtering textbooks.

type kalmanfilter.m

%   Copyright 2010 The MathWorks, Inc.
function y = kalmanfilter(z) 
%#codegen
dt=1;
% Initialize state transition matrix
A=[ 1 0 dt 0 0 0;...     % [x  ]            
       0 1 0 dt 0 0;...     % [y  ]
       0 0 1 0 dt 0;...     % [Vx]
       0 0 0 1 0 dt;...     % [Vy]
       0 0 0 0 1 0 ;...     % [Ax]
       0 0 0 0 0 1 ];       % [Ay]
H = [ 1 0 0 0 0 0; 0 1 0 0 0 0 ];    % Initialize measurement matrix
Q = eye(6);
R = 1000 * eye(2);
persistent x_est p_est                % Initial state conditions
if isempty(x_est)
    x_est = zeros(6, 1);             % x_est=[x,y,Vx,Vy,Ax,Ay]'
    p_est = zeros(6, 6);
end
% 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                % of the function

Load Test Data

The positions of the object to track are recorded as x and y coordinates in a Cartesian space in a MAT file called position_data.mat. The following code loads the MAT file and plots the trace of the positions. The test data includes two sudden shifts or discontinuities in position which are used to check that the Kalman filter can quickly re-adjust and track the object.

load position_data.mat
hold; grid;

Current plot held

for idx = 1: numPts
z = position(:,idx);
plot(z(1), z(2), 'bx');
axis([-1 1 -1 1]);
end
title('Test vector for the Kalman filtering with 2 sudden discontinuities ');
xlabel('x-axis');ylabel('y-axis');
hold;

Current plot released

Inspect and Run the ObjTrack Function

The ObjTrack.m function calls the Kalman filter algorithm and 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.

type ObjTrack

%   Copyright 2010 The MathWorks, Inc.
function ObjTrack(position)
%#codegen
% First, setup the figure
numPts = 300;               % Process and plot 300 samples
figure;hold;grid;            % Prepare plot window
% Main loop
for idx = 1: numPts
    z = position(:,idx);     % Get the input data
    y = kalmanfilter(z);        % Call Kalman filter to estimate the position
    plot_trajectory(z,y);    % Plot the results
end
hold;
end   % of the function

ObjTrack(position)

Current plot held
Current plot released

Generate C Code

The codegen command with the -config:lib option generates C code packaged as a standalone C library.

Because C uses static typing, codegen must determine the properties of all variables in the MATLAB files at compile time. Here, the -args command-line option supplies an example input so that codegen can infer new types based on the input types.

The -report option generates a compilation report that contains a summary of the compilation results and links to generated files. After compiling the MATLAB code, codegen provides a hyperlink to this report.

z = position(:,1);
codegen  -config:lib -report -c kalmanfilter.m -args {z}

Code generation successful: View report

Inspect the Generated Code

The generated C code is in the codegen/lib/kalmanfilter/ folder. The files are:

dir codegen/lib/kalmanfilter/

.                          ..                         _clang-format              buildInfo.mat              codeInfo.mat               codedescriptor.dmr         compileInfo.mat            examples                   eye.c                      eye.h                      html                       interface                  kalmanfilter.c             kalmanfilter.h             kalmanfilter_data.c        kalmanfilter_data.h        kalmanfilter_initialize.c  kalmanfilter_initialize.h  kalmanfilter_rtw.bat       kalmanfilter_rtw.mk        kalmanfilter_rtw.rsp       kalmanfilter_rtw_comp.rsp  kalmanfilter_rtw_ref.rsp   kalmanfilter_terminate.c   kalmanfilter_terminate.h   kalmanfilter_types.h       mldivide.c                 mldivide.h                 rtw_proj.tmw               rtwtypes.h                 setup_mingw.bat            

Inspect the C Code for the kalmanfilter.c Function

type codegen/lib/kalmanfilter/kalmanfilter.c

/*
 * Prerelease License - for engineering feedback and testing purposes
 * only. Not for sale.
 * File: kalmanfilter.c
 *
 * MATLAB Coder version            : 25.1
 * C/C++ source code generated on  : 10-Dec-2024 10:46:46
 */

/* Include Files */
#include "kalmanfilter.h"
#include "eye.h"
#include "kalmanfilter_data.h"
#include "kalmanfilter_initialize.h"
#include "mldivide.h"
#include <emmintrin.h>
#include <string.h>

/* Variable Definitions */
static double x_est[6];

static double p_est[36];

/* Function Definitions */
/*
 * Arguments    : const double z[2]
 *                double y[2]
 * Return Type  : void
 */
void kalmanfilter(const double z[2], double y[2])
{
  static const short R[4] = {1000, 0, 0, 1000};
  static const signed char b_a[36] = {1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
                                      1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0,
                                      0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1};
  static const signed char iv[36] = {1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0,
                                     0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1,
                                     0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1};
  static const signed char c_a[12] = {1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0};
  static const signed char iv1[12] = {1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0};
  double B[36];
  double a[36];
  double p_prd[36];
  double b_B[12];
  double dv[12];
  double x_prd[6];
  double c_B[4];
  double b_z[2];
  double d;
  int B_tmp;
  int i;
  int i1;
  int i2;
  int x_prd_tmp;
  if (!isInitialized_kalmanfilter) {
    kalmanfilter_initialize();
  }
  /*    Copyright 2010 The MathWorks, Inc. */
  /*  Initialize state transition matrix */
  /*      % [x  ] */
  /*      % [y  ] */
  /*      % [Vx] */
  /*      % [Vy] */
  /*      % [Ax] */
  /*  [Ay] */
  /*  Initialize measurement matrix */
  /*  Initial state conditions */
  /*  Predicted state and covariance */
  memset(&x_prd[0], 0, 6U * sizeof(double));
  eye(B);
  memset(&a[0], 0, 36U * sizeof(double));
  for (i = 0; i < 6; i++) {
    for (i1 = 0; i1 < 6; i1++) {
      x_prd_tmp = i1 + 6 * i;
      x_prd[i1] += (double)b_a[x_prd_tmp] * x_est[i];
      d = p_est[x_prd_tmp];
      for (i2 = 0; i2 < 6; i2++) {
        x_prd_tmp = i2 + 6 * i;
        a[x_prd_tmp] += (double)b_a[i2 + 6 * i1] * d;
      }
    }
  }
  /*  Estimation */
  memset(&b_B[0], 0, 12U * sizeof(double));
  for (i2 = 0; i2 < 6; i2++) {
    B_tmp = i2 << 1;
    for (i1 = 0; i1 < 6; i1++) {
      d = 0.0;
      for (i = 0; i < 6; i++) {
        d += a[i2 + 6 * i] * (double)iv[i + 6 * i1];
      }
      x_prd_tmp = i2 + 6 * i1;
      d += B[x_prd_tmp];
      p_prd[x_prd_tmp] = d;
      x_prd_tmp = i1 << 1;
      b_B[B_tmp] += (double)c_a[x_prd_tmp] * d;
      b_B[B_tmp + 1] += (double)c_a[x_prd_tmp + 1] * d;
    }
  }
  for (i2 = 0; i2 < 2; i2++) {
    for (i1 = 0; i1 < 2; i1++) {
      d = 0.0;
      for (i = 0; i < 6; i++) {
        d += b_B[i2 + (i << 1)] * (double)iv1[i + 6 * i1];
      }
      x_prd_tmp = i2 + (i1 << 1);
      c_B[x_prd_tmp] = d + (double)R[x_prd_tmp];
    }
  }
  mldivide(c_B, b_B, dv);
  for (i = 0; i < 2; i++) {
    for (i2 = 0; i2 < 6; i2++) {
      b_B[i2 + 6 * i] = dv[i + (i2 << 1)];
    }
  }
  /*  Estimated state and covariance */
  for (i = 0; i < 2; i++) {
    d = 0.0;
    for (i2 = 0; i2 < 6; i2++) {
      d += (double)c_a[i + (i2 << 1)] * x_prd[i2];
    }
    b_z[i] = z[i] - d;
  }
  memset(&B[0], 0, 36U * sizeof(double));
  for (i2 = 0; i2 < 6; i2++) {
    d = 0.0;
    x_prd_tmp = 6 * i2 + 2;
    B_tmp = 6 * i2 + 4;
    for (i = 0; i < 2; i++) {
      __m128d r;
      __m128d r1;
      __m128d r2;
      d += b_B[i2 + 6 * i] * b_z[i];
      r = _mm_loadu_pd(&b_B[6 * i]);
      r1 = _mm_loadu_pd(&B[6 * i2]);
      r2 = _mm_set1_pd(c_a[i + (i2 << 1)]);
      _mm_storeu_pd(&B[6 * i2], _mm_add_pd(r1, _mm_mul_pd(r, r2)));
      r = _mm_loadu_pd(&b_B[6 * i + 2]);
      r1 = _mm_loadu_pd(&B[x_prd_tmp]);
      _mm_storeu_pd(&B[x_prd_tmp], _mm_add_pd(r1, _mm_mul_pd(r, r2)));
      r = _mm_loadu_pd(&b_B[6 * i + 4]);
      r1 = _mm_loadu_pd(&B[B_tmp]);
      _mm_storeu_pd(&B[B_tmp], _mm_add_pd(r1, _mm_mul_pd(r, r2)));
    }
    x_est[i2] = x_prd[i2] + d;
  }
  /*  Compute the estimated measurements */
  memset(&y[0], 0, sizeof(double) << 1);
  for (i1 = 0; i1 < 6; i1++) {
    for (i = 0; i < 6; i++) {
      d = 0.0;
      for (i2 = 0; i2 < 6; i2++) {
        d += B[i1 + 6 * i2] * p_prd[i2 + 6 * i];
      }
      x_prd_tmp = i1 + 6 * i;
      p_est[x_prd_tmp] = p_prd[x_prd_tmp] - d;
    }
    x_prd_tmp = i1 << 1;
    d = x_est[i1];
    y[0] += (double)c_a[x_prd_tmp] * d;
    y[1] += (double)c_a[x_prd_tmp + 1] * d;
  }
}

/*
 * Arguments    : void
 * Return Type  : void
 */
void kalmanfilter_init(void)
{
  int i;
  for (i = 0; i < 6; i++) {
    x_est[i] = 0.0;
  }
  /*  x_est=[x,y,Vx,Vy,Ax,Ay]' */
  memset(&p_est[0], 0, 36U * sizeof(double));
}

/*
 * File trailer for kalmanfilter.c
 *
 * [EOF]
 */

Accelerate the Execution Speed of the MATLAB Algorithm

You can accelerate the execution speed of the kalmanfilter function that is processing a large data set by using the codegen command to generate a MEX function from the MATLAB code.

Call the kalman_loop Function to Process Large Data Sets

First, run the Kalman algorithm with a large number of data samples in MATLAB. The kalman_loop function runs the kalmanfilter function in a loop. The number of loop iterations is equal to the second dimension of the input to the function.

type kalman_loop

%   Copyright 2010 The MathWorks, Inc.
function y=kalman_loop(z)
% Call Kalman estimator in the loop for large data set testing
%#codegen
[DIM, LEN]=size(z);
y=zeros(DIM,LEN);           % Initialize output
for n=1:LEN                     % Output in the loop
    y(:,n)=kalmanfilter(z(:,n));
end;

Baseline Execution Speed Without Compilation

Now time the MATLAB algorithm. Use the randn command to generate random numbers and create the input matrix position composed of 100,000 samples of (2x1) position vectors. Remove all MEX files from the current folder. Use the MATLAB stopwatch timer (tic and toc commands) to measure how long it takes to process these samples when running the kalman_loop function.

clear mex
delete(['*.' mexext])
position = randn(2,100000);
tic, kalman_loop(position); a=toc;

Generate a MEX Function for Testing

Next, generate a MEX function using the command codegen followed by the name of the MATLAB function kalman_loop. The codegen command generates a MEX function called kalman_loop_mex. You can then compare the execution speed of this MEX function with that of the original MATLAB algorithm.

codegen -args {position} kalman_loop.m

Code generation successful.

Time the MEX Function

Now, time the MEX function kalman_loop_mex. Use the same signal position as before as the input, to ensure a fair comparison of the execution speed.

tic, kalman_loop_mex(position); b=toc;

Comparison of the Execution Speeds

Notice the speed execution difference using a generated MEX function.

display(sprintf('The speedup is %.1f times using the generated MEX over the baseline MATLAB function.',a/b));

The speedup is 6.7 times using the generated MEX over the baseline MATLAB function.