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

Открыть сценарий в реальном времени

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

Предпосылки

Предпосылки для этого примера отсутствуют.

О `kalmanfilter` Функция

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

Сведения об алгоритме оценки фильтра Калмана

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

${\begin{array}{rcl} X & =_{} {X0}_{} + \\ _{} VxdtY_{=} \\ {Y0}_{} & + & _{}_{} VydtVx \\ =_{} & _{Vx0} +_{} \end{array}$ 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` Функция

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

Создать код C

codegen с помощью команды -config:lib создает код C, упакованный как автономная библиотека C.

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

-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

Проверьте код 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. 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. 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.

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

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

Предпосылки

О `kalmanfilter` Функция

Сведения об алгоритме оценки фильтра Калмана

Данные нагрузочного теста

Проверка и запуск `ObjTrack` Функция

Создать код C

Проверка созданного кода

Проверьте код C для `kalmanfilter.c` Функция

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

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

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

Создание функции MEX для тестирования

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

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

Документация по кодеру MATLAB

Поддержка

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

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

Предпосылки

О kalmanfilter Функция

Сведения об алгоритме оценки фильтра Калмана

Данные нагрузочного теста

Проверка и запуск ObjTrack Функция

Создать код C

Проверка созданного кода

Проверьте код C для kalmanfilter.c Функция

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

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

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

Создание функции MEX для тестирования

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

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

Документация по кодеру MATLAB

Поддержка

О `kalmanfilter` Функция

Проверка и запуск `ObjTrack` Функция

Проверьте код C для `kalmanfilter.c` Функция

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