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

Скрипт Open Live Script

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

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

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

О kalmanfilter Функция

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

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

Оценка состояния фильтра Калмана вычисляет радиус-вектор путем вычисления и обновления вектора состояния Кальмана. Вектор состояния задан как 6 1 вектор-столбец, который включает положение (X и Y), скорость (Vx Vy) и ускорение (Ax и Эйе) измерения на 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 Функция

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

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

codegen команда с -config:lib опция генерирует код С, упакованный как автономная библиотека 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

Смотрите код С для 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 состоявший из 100 000 выборок (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.

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

Поддержка

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