Этот пример показывает, как сгенерировать код С для функции Фильтра Калмана MATLAB, kalmanfilter, который оценивает положение перемещения, основанного на объектах на прошлых шумных измерениях. Это также показывает, как сгенерировать MEX-функцию для этого кода MATLAB, чтобы увеличить скорость выполнения алгоритма в MATLAB.
Нет никаких предпосылок для этого примера.
kalmanfilterФункция kalmanfilter предсказывает положение перемещения, основанного на объектах на его прошлых значениях. Это использует средство оценки Фильтра Калмана, рекурсивный адаптивный фильтр, который оценивает состояние динамической системы от ряда шумных измерений. У Кальмана, фильтрующего, есть широкий диапазон приложения в областях, таких как обработка сигналов и обработка изображений, система управления и вычислительные финансы.
Оценка состояния фильтра Калмана вычисляет радиус-вектор путем вычисления и обновления вектора состояния Кальмана. Вектор состояния задан как 6 1 вектор - столбец, который включает положение (x и y), скорость (Vx Vy) и ускорение (Ax и Эйе) измерения на 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Вызовы функции 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_initialize.c .. kalmanfilter_initialize.h buildInfo.mat kalmanfilter_ref.rsp codeInfo.mat kalmanfilter_rtw.mk codedescriptor.dmr kalmanfilter_terminate.c examples kalmanfilter_terminate.h html kalmanfilter_types.h interface rtw_proj.tmw kalmanfilter.c rtwtypes.h kalmanfilter.h
kalmanfilter.ctype codegen/lib/kalmanfilter/kalmanfilter.c/*
* File: kalmanfilter.c
*
* MATLAB Coder version : 4.1
* C/C++ source code generated on : 21-Aug-2018 23:30:29
*/
/* Include Files */
#include <math.h>
#include <string.h>
#include "kalmanfilter.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 i0;
signed char Q[36];
int k;
double x_prd[6];
int i1;
double d0;
int r1;
double S[4];
int r2;
double 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 B[12];
double p_prd[36];
double a21;
static const signed char b_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 };
double a22_tmp;
double a22;
static const signed char b_b[12] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 };
static const signed char c_a[12] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0 };
static const short R[4] = { 1000, 0, 0, 1000 };
double d1;
double Y[12];
double b_z[2];
/* Copyright 2010 The MathWorks, Inc. */
/* Initialize state transition matrix */
/* % [x ] */
/* % [y ] */
/* % [Vx] */
/* % [Vy] */
/* % [Ax] */
/* [Ay] */
/* Initialize measurement matrix */
for (i0 = 0; i0 < 36; i0++) {
Q[i0] = 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 (i0 = 0; i0 < 6; i0++) {
r1 = k + 6 * i0;
a[r1] = 0.0;
d0 = 0.0;
for (i1 = 0; i1 < 6; i1++) {
d0 += (double)b_a[k + 6 * i1] * p_est[i1 + 6 * i0];
}
a[r1] = d0;
x_prd[k] += (double)b_a[r1] * x_est[i0];
}
}
for (i0 = 0; i0 < 6; i0++) {
for (i1 = 0; i1 < 6; i1++) {
d0 = 0.0;
for (r2 = 0; r2 < 6; r2++) {
d0 += a[i0 + 6 * r2] * (double)b[r2 + 6 * i1];
}
r1 = i0 + 6 * i1;
p_prd[r1] = d0 + (double)Q[r1];
}
}
/* Estimation */
for (i0 = 0; i0 < 2; i0++) {
for (i1 = 0; i1 < 6; i1++) {
r1 = i0 + (i1 << 1);
B[r1] = 0.0;
d0 = 0.0;
for (r2 = 0; r2 < 6; r2++) {
d0 += (double)c_a[i0 + (r2 << 1)] * p_prd[i1 + 6 * r2];
}
B[r1] = d0;
}
for (i1 = 0; i1 < 2; i1++) {
d0 = 0.0;
for (r2 = 0; r2 < 6; r2++) {
d0 += B[i0 + (r2 << 1)] * (double)b_b[r2 + 6 * i1];
}
r1 = i0 + (i1 << 1);
S[r1] = d0 + (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[2 + r1];
a22 = S[2 + r2] - a21 * a22_tmp;
for (k = 0; k < 6; k++) {
i0 = k << 1;
d0 = B[r1 + i0];
d1 = (B[r2 + i0] - d0 * a21) / a22;
Y[1 + i0] = d1;
Y[i0] = (d0 - d1 * a22_tmp) / S[r1];
}
for (i0 = 0; i0 < 2; i0++) {
for (i1 = 0; i1 < 6; i1++) {
B[i1 + 6 * i0] = Y[i0 + (i1 << 1)];
}
}
/* Estimated state and covariance */
for (i0 = 0; i0 < 2; i0++) {
d0 = 0.0;
for (i1 = 0; i1 < 6; i1++) {
d0 += (double)c_a[i0 + (i1 << 1)] * x_prd[i1];
}
b_z[i0] = z[i0] - d0;
}
for (i0 = 0; i0 < 6; i0++) {
d0 = B[i0 + 6];
x_est[i0] = x_prd[i0] + (B[i0] * b_z[0] + d0 * b_z[1]);
for (i1 = 0; i1 < 6; i1++) {
r1 = i0 + 6 * i1;
a[r1] = 0.0;
r2 = i1 << 1;
a[r1] = B[i0] * (double)c_a[r2] + d0 * (double)c_a[1 + r2];
}
for (i1 = 0; i1 < 6; i1++) {
d0 = 0.0;
for (r2 = 0; r2 < 6; r2++) {
d0 += a[i0 + 6 * r2] * p_prd[r2 + 6 * i1];
}
r1 = i0 + 6 * i1;
p_est[r1] = p_prd[r1] - d0;
}
}
/* Compute the estimated measurements */
for (i0 = 0; i0 < 2; i0++) {
y[i0] = 0.0;
d0 = 0.0;
for (i1 = 0; i1 < 6; i1++) {
d0 += (double)c_a[i0 + (i1 << 1)] * x_est[i1];
}
y[i0] = d0;
}
/* 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]
*/
Можно ускорить скорость выполнения функции 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-функцию с помощью команды codegen, сопровождаемый под названием функция MATLAB kalman_loop. Команда codegen генерирует MEX-функцию под названием kalman_loop_mex. Можно затем сравнить скорость выполнения этой MEX-функции с тем из исходного алгоритма MATLAB.
codegen -args {position} kalman_loop.m
which kalman_loop_mex/tmp/BR2018bd_939393_64162/mlx_to_docbook1/tpf13fa8cc/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 20.4 times using the generated MEX over the baseline MATLAB function.