发帖
雷达卡
MATLAB课程：代码示例之C Code Generation（三）
C Code Generation for a MATLAB Kalman Filtering Algorithm
This example shows how to generate C code for a MATLAB Kalman filter function,'kalmanfilter', which estimates the position of a moving object based on past noisy measurements. It also shows how to generate a MEX function for this MATLAB code to increase the execution speed of the algorithm in MATLAB.
Prerequisites
There are no prerequisites for this example.
Create a New Folder and Copy Relevant FilesThe following code will create a folder in your current working folder (pwd). The new folder will only contain the files that are relevant for this example. If you do not want to affect the current folder (or if you cannot generate files in this folder), you should change your working folder.
Run Command: Create a New Folder and Copy Relevant Filescoderdemo_setup('coderdemo_kalman_filter');About the 'kalmanfilter' 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 Kalman Filter Estimator AlgorithmThe 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 2-dimensional Cartesian space. Based on the classical laws of motion:
The iterative formula capturing these laws are reflected in the Kalman state transition matrix "A". Note that by writing 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) %#codegendt=1;% Initialize state transition matrixA=[ 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 matrixQ = eye(6);R = 1000 * eye(2);persistent x_est p_est % Initial state conditionsif 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 covariancex_prd = A * x_est;p_prd = A * p_est * A' + Q;% EstimationS = H * p_prd' * H' + R;B = H * p_prd';klm_gain = (S \ B)';% Estimated state and covariancex_est = x_prd + klm_gain * (z - H * x_prd);p_est = p_prd - klm_gain * H * p_prd;% Compute the estimated measurementsy = 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 following code loads the MAT file and plots the trace of the positions. The test data includes two sudden shifts or discontinuities in position which are used to check that the Kalman filter can quickly re-adjust and track the object.
load position.mathold; grid;for idx = 1: numPtsz = position(:,idx);plot(z(1), z(2), 'bx');axis([-1 1 -1 1]);endtitle('Test vector for the Kalman filtering, including 2 sudden discontinuities ');xlabel('x-axis');ylabel('y-axis');hold;Current plot heldCurrent plot released
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 with 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 ObjTrackObjTrack(position)% Copyright 2010 The MathWorks, Inc.function ObjTrack(position)%#codegen% First, setup the figurenumPts = 300; % Process and plot 300 samplesfigure;hold;grid; % Prepare plot window% Main loopfor 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 resultsendhold;end % of the functionCurrent plot heldCurrent plot released
The 'codegen' command with 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, open('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/. kalmanfilter_terminate.c .. kalmanfilter_terminate.h buildInfo.mat kalmanfilter_types.h codeInfo.mat rtGetInf.c examples rtGetInf.h html rtGetNaN.c interface rtGetNaN.h kalmanfilter.c rt_nonfinite.c kalmanfilter.h rt_nonfinite.h kalmanfilter_initialize.c rtw_proj.tmw kalmanfilter_initialize.h rtwtypes.h kalmanfilter_ref.rsp kalmanfilter_rtw.mk
Inspect the C Code for the 'kalmanfilter.c' Functiontype codegen/lib/kalmanfilter/kalmanfilter.c
/* * File: kalmanfilter.c * * MATLAB Coder version : 3.1 * C/C++ source code generated on : 07-Jan-2016 17:25:55 *//* Include Files */#include "rt_nonfinite.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]){ signed char Q[36]; int r2; double a[36]; int k; double p_prd[36]; double x_prd[6]; int i0; double b_a[12]; static const signed char c_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 S[4]; double a21; int r1; 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 a22; static const signed char d_a[12] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0 }; double B[12]; static const short R[4] = { 1000, 0, 0, 1000 }; static const signed char b_b[12] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 }; double b_z[2]; double klm_gain[12]; /* 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; } /* Initial state conditions */ /* Predicted state and covariance */ for (k = 0; k < 6; k++) { Q[k + 6 * k] = 1; x_prd[k] = 0.0; for (r2 = 0; r2 < 6; r2++) { x_prd[k] += (double)c_a[k + 6 * r2] * x_est[r2]; a[k + 6 * r2] = 0.0; for (i0 = 0; i0 < 6; i0++) { a[k + 6 * r2] += (double)c_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++) { b_a[r2 + (i0 << 1)] = 0.0; for (r1 = 0; r1 < 6; r1++) { b_a[r2 + (i0 << 1)] += (double)d_a[r2 + (r1 << 1)] * p_prd[i0 + 6 * r1]; } } for (i0 = 0; i0 < 2; i0++) { a21 = 0.0; for (r1 = 0; r1 < 6; r1++) { a21 += b_a[r2 + (r1 << 1)] * (double)b_b[r1 + 6 * i0]; } S[r2 + (i0 << 1)] = a21 + (double)R[r2 + (i0 << 1)]; } for (i0 = 0; i0 < 6; i0++) { B[r2 + (i0 << 1)] = 0.0; for (r1 = 0; r1 < 6; r1++) { B[r2 + (i0 << 1)] += (double)d_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++) { b_a[1 + (k << 1)] = (B[r2 + (k << 1)] - B[r1 + (k << 1)] * a21) / a22; b_a[k << 1] = (B[r1 + (k << 1)] - b_a[1 + (k << 1)] * S[2 + r1]) / S[r1]; } /* Estimated state and covariance */ for (r2 = 0; r2 < 2; r2++) { a21 = 0.0; for (i0 = 0; i0 < 6; i0++) { klm_gain[i0 + 6 * r2] = b_a[r2 + (i0 << 1)]; a21 += (double)d_a[r2 + (i0 << 1)] * x_prd[i0]; } b_z[r2] = z[r2] - a21; } for (r2 = 0; r2 < 6; r2++) { a21 = 0.0; for (i0 = 0; i0 < 2; i0++) { a21 += klm_gain[r2 + 6 * i0] * b_z[i0]; } x_est[r2] = x_prd[r2] + a21; 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)d_a[r1 + (i0 << 1)]; } } 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)d_a[r2 + (i0 << 1)] * x_est[i0]; } } /* of the function */}/* * Arguments : void * Return Type : void */void kalmanfilter_init(void){ int i; for (i = 0; i < 6; i++) { x_est = 0.0; } /* x_est=[x,y,Vx,Vy,Ax,Ay]' */ memset(&p_est[0], 0, 36U * sizeof(double));}/* * File trailer for kalmanfilter.c * * [EOF] */
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 'kalman_loop' function to Process Large Data SetsFirst, run the Kalman algorithm with 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% 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 outputfor n=1:LEN % Output in the loop y(:,n)=kalmanfilter(z(:,n));end;
Baseline Execution Speed Without Compilation
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 stopwatch timer ('tic' and 'toc' commands) to measure how long it takes to process these samples when running the 'kalman_loop' function.
clear mexdelete(['*.' 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' followed 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 with that of the original MATLAB algorithm.
codegen -args {position} kalman_loop.mwhich kalman_loop_mex/tmp/BR2016ad_325204_137356/tp4846d0fe_fe5e_4cf9_b5b3_639a92caf3e5/coderdemo_kalman_filter/kalman_loop_mex.mexa64
Time 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;Comparison 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 25.4 times using the generated MEX over the baseline MATLAB function.
Cleanup
Remove files and return to original folder
Run Command: Cleanupcleanup
京公网安备 11010802022788号
