C Code Generation For A MATLAB Kalman Filtering Algorithm

3/6/14
C Code Generation for a MATLAB Kalman Filtering Algorithm - MATLAB & Simulink Example - MathWorks India
C Code Generation for a MATLAB Kalman Filtering Algorithm

This example show s how to generate C code for a MATLAB Kalman filter function,'kalmanfilter', w hich estimates the position of a
moving object based on past noisy measurements. It also show s how to generate a MEX function for this MATLAB code to
increase the execution speed of the algorithm in MATLAB.
Prerequisites
To run this example, you must install a C compiler and set it up using the 'mex -setup' command. For more information, see Setting
Up Your C Compiler.
Create a New Folder and Copy Relevant Files
The follow ing code w ill create a folder in your current w orking folder (pw d). The new folder w ill only contain the files that are
relevant for this example. If you do not w ant to affect the current folder (or if you cannot generate files in this folder), you should
change your w orking folder.
Run Com m and: Create a New Folder and Copy Relevant Files
coderdemo_setup('coderdemo_kalman_filter');
About the 'kalm anfilter' Function
The 'kalmanfilter' function predicts the position of a moving object based on its past values. It uses a Kalman filter estimator, a
recursive adaptive filter that estimates the state of a dynamic system from a series of noisy measurements. Kalman filtering has a
broad range of application in areas such as signal and image processing, control design, and computational finance.
About the Kalm an Filter Estim ator Algorithm
The Kalman estimator computes the position vector by computing and updating the Kalman state vector. The state vector is defined
as a 6-by-1 column vector that includes position (x and y), velocity (Vx Vy), and acceleration (Ax and Ay) measurements in a 2dimensional Cartesian space. Based on the classical law s of motion:
The iterative formula capturing these law s are reflected in the Kalman state transition matrix "A". Note that by w riting about 10 lines
of MATLAB code, you can implement the Kalman estimator based on the the theoretical mathematical formula found in many
adaptive filtering textbooks.
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]'
www.mathworks.in/help/coder/examples/c-code-generation-for-a-matlab-kalman-filtering-algorithm.html
1/8
3/6/14
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
Load Test Data
The position of the object to track are recorded as x and y coordinates in a Cartesian space in a MAT file called 'position.mat'. The
follow ing code loads the MAT file and plots the trace of the positions. The test data includes tw o sudden shifts or discontinuities in
position w hich are used to check that the Kalman filter can quickly re-adjust and track the object.
load position.mat
hold; grid;
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, including 2 sudden discontinuities ');
xlabel('x-axis');ylabel('y-axis');
hold;
Current plot held
Current plot released
Inspect and Run the 'ObjTrack.m ' Function
2/8
3/6/14
The 'ObjTrack.m' function calls the Kalman filter algorithm and plots the trajectory of the object in blue and the Kalman filter estimated
position in green. Initially, you see that it takes a short time for the estimated position to converge w ith the actual position of the
object. Then, three sudden shifts in position occur. Each time the Kalman filter readjusts and tracks the object after a few iterations.
type ObjTrack
ObjTrack(position)
%
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
Current plot held
Current plot released
Generate C Code
The 'codegen' command w ith the '-config;lib' option generates C code packaged as a standalone C library.
Because C uses static typing, 'codegen' must determine the properties of all variables in the MATLAB files at compile time. Here, the
'-args' command-line option supplies an example input so that 'codegen' can infer new types based on the input types.
The '-report' option generates a compilation report that contains a summary of the compilation results and links to generated files.
After compiling the MATLAB code, 'codegen' provides a hyperlink to this report.
z = position(:,1);
codegen -config:lib -report -c kalmanfilter.m -args {z}
Code generation successful: To view the report,
3/8
3/6/14
open('C:\TEMP\R2013bd_689_5180\tpad8c65a1_e3df_4668_84da_1aece68d5262\coderdemo_kalman
_filter\codegen\lib\kalmanfilter\html\index.html').
Inspect the Generated Code
The generated C code is in the 'codegen/lib/kalmanfilter/' folder. The files are:
dir codegen/lib/kalmanfilter/
.
..
buildInfo.mat
codeInfo.mat
html
interface
kalmanfilter.c
kalmanfilter.h
kalmanfilter_initialize.c
kalmanfilter_initialize.h
kalmanfilter_ref.rsp
kalmanfilter_rtw.bat
kalmanfilter_rtw.mk
kalmanfilter_terminate.c
kalmanfilter_terminate.h
kalmanfilter_types.h
rtGetInf.c
rtGetInf.h
rtGetNaN.c
rtGetNaN.h
rt_nonfinite.c
rt_nonfinite.h
rtw_proj.tmw
rtwtypes.h
Inspect the C Code for the 'kalm anfilter.c' Function

type codegen/lib/kalmanfilter/kalmanfilter.c
/*
* kalmanfilter.c
*
* Code generation for function 'kalmanfilter'
*
* C source code generated on: Fri Aug 09 01:26:17 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "kalmanfilter.h"
/* Variable Definitions */
static double x_est[6];
static double p_est[36];
/* Function Definitions */
void kalmanfilter(const double z[2], double y[2])
{
signed char Q[36];
int r2;
double a[36];
int k;
double x_prd[6];
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 };
int i0;
double p_prd[36];
double a21;
int r1;
4/8
3/6/14
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 klm_gain[12];
static const signed char c_a[12] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0 };
double S[4];
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 B[12];
double a22;
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 (r2 = 0; r2 < 36; r2++) {
Q[r2] = 0;
}
for (k = 0; k < 6; k++) {
Q[k + 6 * k] = 1;
/* Initial state conditions */
/* Predicted state and covariance */
x_prd[k] = 0.0;
for (r2 = 0; r2 < 6; r2++) {
x_prd[k] += (double)b_a[k + 6 * r2] * x_est[r2];
}
for (r2 = 0; r2 < 6; r2++) {
a[k + 6 * r2] = 0.0;
for (i0 = 0; i0 < 6; i0++) {
a[k + 6 * r2] += (double)b_a[k + 6 * i0] * p_est[i0 + 6 * r2];
}
}
}
for (r2 = 0; r2 < 6; r2++) {
for (i0 = 0; i0 < 6; i0++) {
a21 = 0.0;
for (r1 = 0; r1 < 6; r1++) {
a21 += a[r2 + 6 * r1] * (double)b[r1 + 6 * i0];
}
p_prd[r2 + 6 * i0] = a21 + (double)Q[r2 + 6 * i0];
}
}
/* Estimation */
for (r2 = 0; r2 < 2; r2++) {
for (i0 = 0; i0 < 6; i0++) {
5/8
3/6/14
klm_gain[r2 + (i0 << 1)] = 0.0;

for (r1 = 0; r1 < 6; r1++) {
klm_gain[r2 + (i0 << 1)] += (double)c_a[r2 + (r1 << 1)] * p_prd[i0 + 6 *
r1];
}
}
}
for (r2 = 0; r2 < 2; r2++) {
for (i0 = 0; i0 < 2; i0++) {
a21 = 0.0;
for (r1 = 0; r1 < 6; r1++) {
a21 += klm_gain[r2 + (r1 << 1)] * (double)b_b[r1 + 6 * i0];
}
S[r2 + (i0 << 1)] = a21 + (double)R[r2 + (i0 << 1)];
}
}
for (r2 = 0; r2 < 2; r2++) {
for (i0 = 0; i0 < 6; i0++) {
B[r2 + (i0 << 1)] = 0.0;
for (r1 = 0; r1 < 6; r1++) {
B[r2 + (i0 << 1)] += (double)c_a[r2 + (r1 << 1)] * p_prd[i0 + 6 * r1];
}
}
}
if (fabs(S[1]) > fabs(S[0])) {
r1 = 1;
r2 = 0;
} else {
r1 = 0;
r2 = 1;
}
a21 = S[r2] / S[r1];
a22 = S[2 + r2] - a21 * S[2 + r1];
for (k = 0; k < 6; k++) {
Y[1 + (k << 1)] = (B[r2 + (k << 1)] - B[r1 + (k << 1)] * a21) / a22;
Y[k << 1] = (B[r1 + (k << 1)] - Y[1 + (k << 1)] * S[2 + r1]) / S[r1];
}
for (r2 = 0; r2 < 2; r2++) {
for (i0 = 0; i0 < 6; i0++) {
klm_gain[i0 + 6 * r2] = Y[r2 + (i0 << 1)];
}
}
/* Estimated state and covariance */
for (r2 = 0; r2 < 2; r2++) {
a21 = 0.0;
for (i0 = 0; i0 < 6; i0++) {
a21 += (double)c_a[r2 + (i0 << 1)] * x_prd[i0];
}
b_z[r2] = z[r2] - a21;
}
for (r2 = 0; r2 < 6; r2++) {
a21 = 0.0;
6/8
3/6/14
for (i0 = 0; i0 < 2; i0++) {

a21 += klm_gain[r2 + 6 * i0] * b_z[i0];
}
x_est[r2] = x_prd[r2] + a21;
}
for (r2 = 0; r2 < 6; r2++) {
for (i0 = 0; i0 < 6; i0++) {
a[r2 + 6 * i0] = 0.0;
for (r1 = 0; r1 < 2; r1++) {
a[r2 + 6 * i0] += klm_gain[r2 + 6 * r1] * (double)c_a[r1 + (i0 << 1)];
}
}
}
for (r2 = 0; r2 < 6; r2++) {
for (i0 = 0; i0 < 6; i0++) {
a21 = 0.0;
for (r1 = 0; r1 < 6; r1++) {
a21 += a[r2 + 6 * r1] * p_prd[r1 + 6 * i0];
}
p_est[r2 + 6 * i0] = p_prd[r2 + 6 * i0] - a21;
}
}
/* Compute the estimated measurements */
for (r2 = 0; r2 < 2; r2++) {
y[r2] = 0.0;
for (i0 = 0; i0 < 6; i0++) {
y[r2] += (double)c_a[r2 + (i0 << 1)] * x_est[i0];
}
}
/* of the function */
}
void kalmanfilter_init(void)
{
int i;
for (i = 0; i < 6; i++) {
x_est[i] = 0.0;
}
memset(&p_est[0], 0, 36U * sizeof(double));
}
/* End of code generation (kalmanfilter.c) */
Accelerate the Execution Speed of the MATLAB Algorithm
You can accelerate the execution speed of the 'kalmanfilter' function that is processing a large data set by using the 'codegen'
command to generate a MEX function from the MATLAB code.
Call the 'kalm an_loop' function to Process Large Data Sets
First, run the Kalman algorithm w ith a large number of data samples in MATLAB. The 'kalman_loop' function runs the 'kalmanfilter'
function in a loop. The number of loop iterations is equal to the second dimension of the input to the function.
type kalman_loop
7/8
3/6/14
%
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;
Baseline Execution Speed Without Com pilation
Now time the MATLAB algorithm. Use the 'randn' command to generate random numbers and create the input matrix 'position'
composed of 100,000 samples of (2x1) position vectors. Remove all MEX files from the current folder. Use the MATLAB stopw atch
timer ('tic' and 'toc' commands) to measure how long it takes to process these samples w hen running the 'kalman_loop' function.
clear mex
delete(['*.' mexext])
position =randn(2,100000);
tic, kalman_loop(position); a=toc;
Generate a MEX Function for Testing
Next, generate a MEX function using the command 'codegen' follow ed by the name of the MATLAB function 'kalman_loop'. The
'codegen' command generates a MEX function called 'kalman_loop_mex'. You can then compare the execution speed of this MEX
function w ith that of the original MATLAB algorithm.
codegen -args {position} kalman_loop.m
which kalman_loop_mex
C:\TEMP\R2013bd_689_5180\tpad8c65a1_e3df_4668_84da_1aece68d5262\coderdemo_kalman_filte
r\kalman_loop_mex.mexw64
Tim e the MEX Function
Now , time the MEX function 'kalman_loop_mex'. Use the same signal 'position' as before as the input, to ensure a fair comparison of
the execution speed.
tic, kalman_loop_mex(position); b=toc;
Com parison of the Execution Speeds
Notice the speed execution difference using a generated MEX function.
display(sprintf('The speedup is %.1f times using the generated MEX over the baseline
MATLAB function.',a/b));
The speedup is 63.4 times using the generated MEX over the baseline MATLAB function.
Cleanup
Remove files and return to original folder
Run Com m and: Cleanup
cleanup
8/8

C Code Generation For A MATLAB Kalman Filtering Algorithm - MATLAB & Simulink Example - MathWorks India

Transféré par

Informations du document

Titre original

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

C Code Generation For A MATLAB Kalman Filtering Algorithm - MATLAB & Simulink Example - MathWorks India

Transféré par

Droits d'auteur :

Formats disponibles

3/6/14

p_est = zeros(6, 6);

Inspect and Run the 'ObjTrack.m ' Function

Inspect the C Code for the 'kalm anfilter.c' Function

static const signed char b[36] = { 1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1,

klm_gain[r2 + (i0 << 1)] = 0.0;

for (i0 = 0; i0 < 2; i0++) {

Vous aimerez peut-être aussi