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

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

Предпосылки

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

О `kalmanfilter` Функция

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

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

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

${\begin{array}{rcl} X & = & X_{0} + V_{x} d t \\ Y & = & Y_{0} + V_{y} d t \\ V_{x} & = & V_{x 0} + A_{x} d t \\ V_{y} & = & V_{y 0} + A_{y} d t \end{array}$

Итеративная формула, получая эти законы отражается в матрице переходов Кальмана "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;

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

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_data.h        
..                         kalmanfilter_initialize.c  
buildInfo.mat              kalmanfilter_initialize.h  
codeInfo.mat               kalmanfilter_ref.rsp       
codedescriptor.dmr         kalmanfilter_rtw.mk        
examples                   kalmanfilter_terminate.c   
html                       kalmanfilter_terminate.h   
interface                  kalmanfilter_types.h       
kalmanfilter.c             rtw_proj.tmw               
kalmanfilter.h             rtwtypes.h                 
kalmanfilter_data.c

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

type codegen/lib/kalmanfilter/kalmanfilter.c

/*
 * File: kalmanfilter.c
 *
 * MATLAB Coder version            : 4.3
 * C/C++ source code generated on  : 23-Dec-2019 07:58:04
 */

/* 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])
{
  int i;
  signed char Q[36];
  int k;
  double x_prd[6];
  int i1;
  double d;
  int r1;
  double S[4];
  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 };

  int r2;
  double b_a[36];
  static const signed char b[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 };

  double p_prd[36];
  double a21;
  double a22_tmp;
  double B[12];
  static const signed char c_a[12] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0 };

  double a22;
  static const signed char b_b[12] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 };

  static const short R[4] = { 1000, 0, 0, 1000 };

  double Y[12];
  double b_z[2];
  if (isInitialized_kalmanfilter == false) {
    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 (i1 = 0; i1 < 6; i1++) {
        d += (double)a[k + 6 * i1] * p_est[i1 + 6 * i];
      }

      b_a[r1] = d;
    }
  }

  for (i = 0; i < 6; i++) {
    for (i1 = 0; i1 < 6; i1++) {
      d = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        d += b_a[i + 6 * r1] * (double)b[r1 + 6 * i1];
      }

      r1 = i + 6 * i1;
      p_prd[r1] = d + (double)Q[r1];
    }
  }

  /*  Estimation */
  for (i = 0; i < 2; i++) {
    for (i1 = 0; i1 < 6; i1++) {
      d = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        d += (double)c_a[i + (r1 << 1)] * p_prd[i1 + 6 * r1];
      }

      B[i + (i1 << 1)] = d;
    }

    for (i1 = 0; i1 < 2; i1++) {
      d = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        d += B[i + (r1 << 1)] * (double)b_b[r1 + 6 * i1];
      }

      r1 = i + (i1 << 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;
    i1 = r1 + i;
    d = (B[r2 + i] - B[i1] * a21) / a22;
    Y[i + 1] = d;
    Y[i] = (B[i1] - d * a22_tmp) / S[r1];
  }

  for (i = 0; i < 2; i++) {
    for (i1 = 0; i1 < 6; i1++) {
      B[i1 + 6 * i] = Y[i + (i1 << 1)];
    }
  }

  /*  Estimated state and covariance */
  for (i = 0; i < 2; i++) {
    d = 0.0;
    for (i1 = 0; i1 < 6; i1++) {
      d += (double)c_a[i + (i1 << 1)] * x_prd[i1];
    }

    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 (i1 = 0; i1 < 6; i1++) {
      r1 = i1 << 1;
      b_a[i + 6 * i1] = B[i] * (double)c_a[r1] + d * (double)c_a[r1 + 1];
    }

    for (i1 = 0; i1 < 6; i1++) {
      d = 0.0;
      for (r1 = 0; r1 < 6; r1++) {
        d += b_a[i + 6 * r1] * p_prd[r1 + 6 * i1];
      }

      r1 = i + 6 * i1;
      p_est[r1] = p_prd[r1] - d;
    }
  }

  /*  Compute the estimated measurements */
  for (i = 0; i < 2; i++) {
    d = 0.0;
    for (i1 = 0; i1 < 6; i1++) {
      d += (double)c_a[i + (i1 << 1)] * x_est[i1];
    }

    y[i] = d;
  }

  /*  of the function */
}

/*
 * 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
which kalman_loop_mex

/tmp/BR2019bd_1276998_130124/mlx_to_docbook1/tpdb2c8a59/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 18.2 times using the generated MEX over the baseline MATLAB function.

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