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

Скрипт Open Live Script

В этом примере показано, как сгенерировать код С для функции Фильтра Калмана 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.c        
..                         kalmanfilter_data.h        
.gitignore                 kalmanfilter_initialize.c  
buildInfo.mat              kalmanfilter_initialize.h  
codeInfo.mat               kalmanfilter_ref.rsp       
codedescriptor.dmr         kalmanfilter_rtw.mk        
compileInfo.mat            kalmanfilter_terminate.c   
defines.txt                kalmanfilter_terminate.h   
examples                   kalmanfilter_types.h       
html                       rtw_proj.tmw               
interface                  rtwtypes.h                 
kalmanfilter.c             
kalmanfilter.h

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

type codegen/lib/kalmanfilter/kalmanfilter.c

/*
 * File: kalmanfilter.c
 *
 * MATLAB Coder version            : 5.1
 * C/C++ source code generated on  : 17-Aug-2020 20:26:17
 */

/* 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;
  }

  /*  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/BR2020bd_1459859_105924/mlx_to_docbook1/tp0577354d/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 20.6 times using the generated MEX over the baseline MATLAB function.

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

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

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

О `kalmanfilter` Функция

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

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

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

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

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

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

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

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

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

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

Время MEX-функция

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

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

Поддержка

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

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

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

О kalmanfilter Функция

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

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

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

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

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

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

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

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

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

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

Время MEX-функция

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

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

Поддержка

О `kalmanfilter` Функция

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

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

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