В этом примере показано, как сгенерировать код С для функции фильтра MATLAB ® Kalman, kalmanfilter, который оценивает положение движущегося объекта на основе прошлых шумных измерений. Это также показывает, как сгенерировать MEX-функцию для этого кода MATLAB, чтобы увеличить скорость выполнения алгоритма в MATLAB.
Для этого примера нет необходимых условий.
kalmanfilter ФункцияThe kalmanfilter функция предсказывает положение движущегося объекта на основе его прошлых значений. Он использует оценщик фильтра Калмана, рекурсивный адаптивный фильтр, который оценивает состояние динамической системы из ряда шумных измерений. Фильтрация Калмана имеет широкую область значений применений в таких областях, как обработка сигналов и изображений, система управления и вычислительные финансы.
Вектор положения вычисляется Оценкой состояния фильтра Калмана путем вычисления и обновления вектора состояния Калмана. Вектор состояния задан как вектор-столбец 6 на 1, которая включает в себя измерения положения (x и y), скорости (Vx Vy) и ускорения (Ax и Ay) в 2-мерном декартовом пространстве. На основе классических законов движения:
Итерационная формула, захватывающая эти законы, отражена в матрице переходов состояний Калмана «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 Функция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.](../../examples/coder/win64/CCodeGenerationForAMATLABKalmanFilteringAlgorithmExample_02.png)
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]
*/
Можно ускорить скорость выполнения 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-функцию с помощью команды 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-функция 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.