Генерация кода C для алгоритма фильтрации MATLAB Калмана

Открыть Live Script

В этом примере показано, как сгенерировать код С для функции фильтра MATLAB ® Kalman, kalmanfilter, который оценивает положение движущегося объекта на основе прошлых шумных измерений. Это также показывает, как сгенерировать MEX-функцию для этого кода MATLAB, чтобы увеличить скорость выполнения алгоритма в MATLAB.

Необходимые условия

Для этого примера нет необходимых условий.

О kalmanfilter Функция

The kalmanfilter функция предсказывает положение движущегося объекта на основе его прошлых значений. Он использует оценщик фильтра Калмана, рекурсивный адаптивный фильтр, который оценивает состояние динамической системы из ряда шумных измерений. Фильтрация Калмана имеет широкую область значений применений в таких областях, как обработка сигналов и изображений, система управления и вычислительные финансы.

Об алгоритме оценки фильтра Калмана

Вектор положения вычисляется Оценкой состояния фильтра Калмана путем вычисления и обновления вектора состояния Калмана. Вектор состояния задан как вектор-столбец 6 на 1, которая включает в себя измерения положения (x и y), скорости (Vx Vy) и ускорения (Ax и Ay) в 2-мерном декартовом пространстве. На основе классических законов движения:

{X=X0+VxdtY=Y0+VydtVx=Vx0+AxdtVy=Vy0+Aydt

Итерационная формула, захватывающая эти законы, отражена в матрице переходов состояний Калмана «A». Обратите внимание, что, написав около 10 линии кода MATLAB, можно реализовать оценка состояния фильтра Калмана на основе теоретической математической формулы, найденной во многих учебниках адаптивной фильтрации.

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

Загрузка тестовых данных

Положение объекта для отслеживания записывается как координаты x и y в декартовом пространстве в файле MAT под названием position_data.mat. Следующий код загружает файл MAT и строит график трассировки позиций. Тестовые данные включают в себя два внезапных сдвига или разрывы в положении, которые используются для проверки того, что фильтр Калмана может быстро перерегулировать и отслеживать объект.

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;

Figure contains an axes. The axes with title Test vector for the Kalman filtering with 2 sudden discontinuities contains 310 objects of type line.

Current plot released

Смотрите и запустите ObjTrack Функция

The ObjTrack.m функция вызывает алгоритм фильтра Калмана и строит график траектории объекта в синем и оцененном положении фильтра Калмана в зеленом цвете. Первоначально вы видите, что для сходимости предполагаемой позиции с фактическим положением объекта требуется короткое время. Затем происходят три внезапных сдвигов положения. Каждый раз фильтр Калмана корректирует и отслеживает объект после нескольких итераций.

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

Figure contains an axes. The axes with title Trajectory of object [blue] its Kalman estimate [green] contains 600 objects of type line.

Current plot released

Сгенерируйте код С

The codegen команда со -config:lib опция генерирует код С, упакованный как автономная библиотека C.

Поскольку на C используется статическое типирование, codegen необходимо определить свойства всех переменных в файлах MATLAB во время компиляции. Вот, -args опция командной строки предоставляет пример входа, так что codegen можно вывести новые типы на основе типов входа.

The -report опция генерирует отчет компиляции, содержащий сводные данные результатов компиляции и ссылки на сгенерированные файлы. После компиляции кода MATLAB, codegen содержит гиперссылку на этот отчет.

z = position(:,1);
codegen  -config:lib -report -c kalmanfilter.m -args {z}
Code generation successful: To view the report, open('codegen/lib/kalmanfilter/html/report.mldatx').

Смотрите сгенерированный код

Сгенерированный код C находится в codegen/lib/kalmanfilter/ папка. Файлы:

dir codegen/lib/kalmanfilter/
.                          kalmanfilter.h             
..                         kalmanfilter_data.c        
.gitignore                 kalmanfilter_data.h        
_clang-format              kalmanfilter_initialize.c  
buildInfo.mat              kalmanfilter_initialize.h  
codeInfo.mat               kalmanfilter_rtw.mk        
codedescriptor.dmr         kalmanfilter_terminate.c   
compileInfo.mat            kalmanfilter_terminate.h   
defines.txt                kalmanfilter_types.h       
examples                   rtw_proj.tmw               
html                       rtwtypes.h                 
interface                  
kalmanfilter.c

Смотрите код С для kalmanfilter.c Функция

type codegen/lib/kalmanfilter/kalmanfilter.c
/*
 * File: kalmanfilter.c
 *
 * MATLAB Coder version            : 5.2
 * C/C++ source code generated on  : 21-Apr-2021 01:20:55
 */

/* Include Files */
#include "kalmanfilter.h"
#include "kalmanfilter_data.h"
#include "kalmanfilter_initialize.h"
#include <math.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 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_a[36];
  double p_prd[36];
  double B[12];
  double Y[12];
  double x_prd[6];
  double S[4];
  double b_z[2];
  double a21;
  double a22;
  double a22_tmp;
  double d;
  double d1;
  int i;
  int k;
  int r1;
  int r2;
  signed char Q[36];
  if (!isInitialized_kalmanfilter) {
    kalmanfilter_initialize();
  }
  /*    Copyright 2010 The MathWorks, Inc. */
  /*  Initialize state transition matrix */
  /*      % [x  ] */
  /*      % [y  ] */
  /*      % [Vx] */
  /*      % [Vy] */
  /*      % [Ax] */
  /*  [Ay] */
  /*  Initialize measurement matrix */
  for (i = 0; i < 36; i++) {
    Q[i] = 0;
  }
  /*  Initial state conditions */
  /*  Predicted state and covariance */
  for (k = 0; k < 6; k++) {
    Q[k + 6 * k] = 1;
    x_prd[k] = 0.0;
    for (i = 0; i < 6; i++) {
      r1 = k + 6 * i;
      x_prd[k] += (double)a[r1] * x_est[i];
      d = 0.0;
      for (r2 = 0; r2 < 6; r2++) {
        d += (double)a[k + 6 * r2] * p_est[r2 + 6 * i];
      }
      b_a[r1] = d;
    }
  }
  for (i = 0; i < 6; i++) {
    for (r2 = 0; r2 < 6; r2++) {
      d = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        d += b_a[i + 6 * r1] * (double)iv[r1 + 6 * r2];
      }
      r1 = i + 6 * r2;
      p_prd[r1] = d + (double)Q[r1];
    }
  }
  /*  Estimation */
  for (i = 0; i < 2; i++) {
    for (r2 = 0; r2 < 6; r2++) {
      d = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        d += (double)c_a[i + (r1 << 1)] * p_prd[r2 + 6 * r1];
      }
      B[i + (r2 << 1)] = d;
    }
    for (r2 = 0; r2 < 2; r2++) {
      d = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        d += B[i + (r1 << 1)] * (double)iv1[r1 + 6 * r2];
      }
      r1 = i + (r2 << 1);
      S[r1] = d + (double)R[r1];
    }
  }
  if (fabs(S[1]) > fabs(S[0])) {
    r1 = 1;
    r2 = 0;
  } else {
    r1 = 0;
    r2 = 1;
  }
  a21 = S[r2] / S[r1];
  a22_tmp = S[r1 + 2];
  a22 = S[r2 + 2] - a21 * a22_tmp;
  for (k = 0; k < 6; k++) {
    i = k << 1;
    d = B[r1 + i];
    d1 = (B[r2 + i] - d * a21) / a22;
    Y[i + 1] = d1;
    Y[i] = (d - d1 * a22_tmp) / S[r1];
  }
  for (i = 0; i < 2; i++) {
    for (r2 = 0; r2 < 6; r2++) {
      B[r2 + 6 * i] = Y[i + (r2 << 1)];
    }
  }
  /*  Estimated state and covariance */
  for (i = 0; i < 2; i++) {
    d = 0.0;
    for (r2 = 0; r2 < 6; r2++) {
      d += (double)c_a[i + (r2 << 1)] * x_prd[r2];
    }
    b_z[i] = z[i] - d;
  }
  for (i = 0; i < 6; i++) {
    d = B[i + 6];
    x_est[i] = x_prd[i] + (B[i] * b_z[0] + d * b_z[1]);
    for (r2 = 0; r2 < 6; r2++) {
      r1 = r2 << 1;
      b_a[i + 6 * r2] = B[i] * (double)c_a[r1] + d * (double)c_a[r1 + 1];
    }
    for (r2 = 0; r2 < 6; r2++) {
      d = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        d += b_a[i + 6 * r1] * p_prd[r1 + 6 * r2];
      }
      r1 = i + 6 * r2;
      p_est[r1] = p_prd[r1] - d;
    }
  }
  /*  Compute the estimated measurements */
  for (i = 0; i < 2; i++) {
    d = 0.0;
    for (r2 = 0; r2 < 6; r2++) {
      d += (double)c_a[i + (r2 << 1)] * x_est[r2];
    }
    y[i] = 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]
 */

Ускорение скорости выполнения алгоритма MATLAB

Можно ускорить скорость выполнения kalmanfilter функция, которая обрабатывает большой набор данных при помощи codegen команда для генерации MEX-функции из кода MATLAB.

Вызовите kalman_loop Функция для обработки больших наборов данных

Сначала запустите алгоритм Калмана с большим количеством выборок данных в MATLAB. The kalman_loop функция запускает kalmanfilter функция в цикле. Количество итераций цикла равно второму измерению входа функции.

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;

Базовая скорость выполнения без компиляции

Сейчас алгоритм MATLAB. Используйте randn команда для генерации случайных чисел и создания матрицы входа position состоит из 100000 выборок (2x1) векторов положения. Удалите все файлы MEX из текущей папки. Используйте таймер секундомера MATLAB (tic и toc команды), чтобы измерить, сколько времени требуется для обработки этих выборок при запуске kalman_loop функция.

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

Сгенерируйте MEX-функцию для проверки

Далее сгенерируйте MEX-функцию с помощью команды codegen далее указывается имя функции MATLAB kalman_loop. The codegen команда генерирует MEX-функцию, вызываемую kalman_loop_mex. Затем можно сравнить скорость выполнения этой MEX-функции с частотой выполнения исходного алгоритма MATLAB.

codegen -args {position} kalman_loop.m
Code generation successful.

which kalman_loop_mex
/tmp/BR2021ad_1655202_180016/mlx_to_docbook1/tp78faea36/coder-ex53054096/kalman_loop_mex.mexa64

Время выполнения MEX-функции

Теперь время, когда MEX-функция kalman_loop_mex. Используйте тот же сигнал position как и прежде в качестве входа, для обеспечения справедливого сравнения скорости выполнения. 

tic, kalman_loop_mex(position); b=toc;

Сравнение скоростей выполнения

Заметьте различие в скорости выполнения с помощью сгенерированной MEX-функции.

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

Документация MATLAB Coder

Поддержка

Для просмотра документации необходимо авторизоваться на сайте