CloudRunner

... share your algorithms as a web service

Algorithm: Kernel-SME filter

Description:
Prototype implementation of the Kernel-SME filter for a simple stationary scenario with 8 closely-spaced targets.
Paper: Marcus Baum and Uwe D. Hanebeck, "The Kernel-SME Filter for Multiple Target Tracking", arXiv preprint: Systems and Control (cs.SY), 2012
Tags: Kernel-SME filter Multiple Target Tracking
Usage: Algorithm is public.
Viewed 6937 times, called 174 times
Upload:
Empty star Empty star Empty star Empty star Empty star
0 votes
Marcus Baum
03/04/2013 1:19 p.m. (version #1)

Run Algorithm

: Measurement noise for a target (2x2 covariance matrix)
: Uncertainty of the initial target estimate (2x2 covariance matrix)
: Kernel width
: Number of time steps simulated
: Allow cached result?
Close

Please Wait

Computation is running...

Plots

showing plot # of

Result Value(s)


		

Resource Usage

Execution of the algorithm took seconds.
Peak memory usage for running the algorithm was kilobytes (total usage: kilobytes).

Run ID / Link

ID of this run:
Link:

Result from Cache

Result has been delivered from cache and computed on (UTC).

Matlab log

(show)

		

Error

Using this algorithm in your local MATLAB environment is easy: Click here for instructions!

Usage Instructions for CloudRunner Client

  1. Download the CloudRunner Client by clicking here and place the downloaded file in your MATLAB working directory.

  2. Inside MATLAB, initialize the CloudRunner Client by calling CloudRunner:
    >> CloudRunner

    A login dialog will prompt for your CloudRunner mail address and password. For a start, you can leave the dialog empty and just click "Connect".

    Alternatively, you can provide the login credentials (or empty strings to skip login) as a parameter and hence skip the login dialog. This is useful when using CloudRunner in non-interactive scripts.
    >> CloudRunner('mail@example.com', 'password')

  3. Select this algorithm by its URL. Selecting an algorithm creates the lambda function that proxies calls to the algorithm to the server for execution:
    >> CloudRunnerSelect('http://www.cloudrunner.eu/algorithm/117/kernel-sme-filter/version/1/')

    For the sake of convenience, you can also use the algorithm ID instead of its URL for public algorithms.

  4. Call functions from the algorithm like any regular local function.

Note: You can find further information on the help page.

Source Code

File:

  1 function  KernelSME_Filter_Demo(measNoise, xCovPriorSingle, kernelwidth,nr_timesteps)
  2 
  3 % Implementation of the Kernel-SME filter according to 
  4 % 
  5 % Marcus Baum, Uwe D. Hanebeck,
  6 % The Kernel-SME Filter for Multiple Target Tracking,
  7 % arXiv preprint: Systems and Control (cs.SY), 2012.
  8 % 
  9 % Note that the implementation is not optimized for speed, 
 10 % e.g., a smarter evaluation of Gaussian densities can significantly improve the speed.
 11 % 
 12 % AUTHOR: Marcus Baum, 2013
 13 
 14 rng('shuffle');
 15 if nargin== 0
 16     nr_timesteps = 15;
 17     measNoise = 0.5  *eye(2) ;
 18     xCovPriorSingle = 0.7 *eye(2);
 19     kernelwidth =2;
 20 end
 21 
 22 % Initialize true target positions
 23 single_xTrue =[6.8042    5.3511    0.6489   -0.8042    3.0000    4.7321    1.2679    3.0000;
 24     4.2361   -0.2361   -0.2361    4.2361    7.0000    2.0000    2.0000    5.0000];
 25 nr_targets = size(single_xTrue,2);
 26 xTrue = reshape(single_xTrue,2*nr_targets,1);
 27 
 28 % Initial estimate: Prior estimate for a target is sampled 
 29 % from a Gaussian with covariance matrix xCovPriorSingle
 30 xCov = [];
 31 for i=1:nr_targets
 32   xCov = blkdiag(xCov, xCovPriorSingle);
 33 end
 34 xEst =xTrue + mvnrnd(zeros(1,2*nr_targets)' , xCov)';
 35 
 36 %%% Initialize figures
 37 sp(1) = subplot(1,2,1)    ;
 38 setAxisProps('Measurements');
 39 sp(2) = subplot(1,2,2)  ; 
 40 setAxisProps('True and Estimated Targets');
 41 plot(single_xTrue(1,:),single_xTrue(2,:),'o','Color','k','MarkerSize',2,'LineWidth',1);
 42 single_xEst = reshape(xEst,2, nr_targets);
 43 last_estimate = plot(single_xEst(1,:),single_xEst(2,:), 'ro');   
 44 drawnow
 45 
 46  
 47 %%%%%%%%%%%%%%%%%%% Main loop 
 48  
 49 for k = 1:nr_timesteps
 50     %%% Generate measurements
 51     meas = zeros(2,nr_targets);
 52     for t= 1:nr_targets
 53         meas(1:2,t) = mvnrnd(single_xTrue(:,t),measNoise );
 54     end
 55     
 56     %%%%%%%%%%% Kernel-SME filter update
 57     
 58     %%% Step 1: Calculate test vectors
 59     testpos = calctestpos(meas,kernelwidth);
 60     
 61     %%% Step 2: Calculate pseudo-measurement
 62     pseudoMeas = calcPseudoMeas(meas, testpos,kernelwidth, nr_targets);
 63            
 64     %%% Step 3: Calculate moments for LMMSE update
 65     [mu_s sigma_s sigma_xs] = calcMoments(xEst, xCov, eye(2), measNoise, testpos, kernelwidth,nr_targets);
 66             
 67     %%% Step 4: Perform LMMSE update
 68     xEst = xEst + sigma_xs * sigma_s^-1 * (pseudoMeas-mu_s);
 69     xCov = xCov - sigma_xs * sigma_s^-1 * sigma_xs';
 70     
 71     %%%%%%%%%%% Plots
 72     subplot(sp(1));
 73     plot(meas(1,:),meas(2,:),'x')
 74     subplot(sp(2))
 75     single_xEst = reshape(xEst,2, nr_targets);
 76     delete(last_estimate)
 77     last_estimate = plot(single_xEst(1,:),single_xEst(2,:), 'ro','MarkerSize',6);   
 78     drawnow
 79 end
 80 end
 81 
 82 
 83 function ys =  calcPseudoMeas(meas,testpos, kernelwidth,nr_targets)
 84 ys=zeros(size(testpos,2),1);
 85 for j = 1:size(testpos,2)
 86     ys(j) = 0;
 87     for k = 1: nr_targets
 88        ys(j) = ys(j)+ mvnpdf( testpos(:,j)', meas(:,k)', kernelwidth*eye(length(meas(:,k))));
 89     end
 90 end
 91 end
 92 
 93 
 94 function testpos= calctestpos(single_xTrue,kernelwidth)
 95 nr_targets = size(single_xTrue,2);
 96 testpos= [];
 97 for i =1:nr_targets
 98     xt=    single_xTrue(:,i);
 99     d = sqrt(2*kernelwidth);
100     testpos = [testpos; xt(1) xt(2); xt(1) xt(2)+d; xt(1) xt(2)-d; xt(1)+d xt(2); xt(1)-d xt(2)];
101 end
102 testpos=testpos';
103 end
104 
105 
106 function  [mu_s sigma_s sigma_xs] =calcMoments(xEst, xCov, H, measNoise, testpos, kernelwidth,nr_targets)
107 x_dim = length(xEst)/nr_targets;
108 gti = @(target)((target-1)*x_dim+1):(target*x_dim);
109 
110 %%% Step 3a: Pre-calculate required mvnpdf evaluations
111 P =zeros(size(testpos,2), nr_targets );
112 for l =1:nr_targets
113    s_temp = H* xCov(gti(l),gti(l)) *H'  +measNoise   +kernelwidth *eye(2);
114    mu_temp = (H *xEst(gti(l)))';
115    for i=1:size(testpos,2) 
116       P(i,l) =  mvnpdf(testpos(:,i)', mu_temp ,  s_temp) ;
117    end
118 end
119  
120 Pij =zeros(size(testpos,2),size(testpos,2), nr_targets );
121 for l =1:nr_targets
122   s_temp =   H* xCov(gti(l),gti(l))    *H'  +measNoise   +0.5*kernelwidth *eye(2);
123   mu_temp =   (H *xEst(gti(l)))';
124   for i=1:size(testpos,2) 
125     for j=1:size(testpos,2)
126       Pij(i,j,l) =  mvnpdf(0.5*(testpos(:,i)'+testpos(:,j)'),  mu_temp,  s_temp) ;
127     end
128   end
129 end
130 
131 %%% Step 3b: Calculate mean of predicted pseudo-measurement
132 mu_s= zeros(size(testpos,2),1);
133 for i=1:size(testpos,2) 
134     mu_s(i) = sum(P(i,:));
135 end
136 
137 %%%% Step 3c: Calculate covariance of predicted pseudo-measurement
138 sigma_s =zeros(size(testpos,2),size(testpos,2) );
139  for i= 1:size(testpos,2)
140      for j= 1:size(testpos,2)
141          M = zeros(1, nr_targets);
142          for m=1:nr_targets
143             M(m) = sum(P(j,1:end));
144          end         
145          for m=1:nr_targets 
146              sigma_s(i,j) = sigma_s(i,j)+ P(i,m) * (M(m)-P(j,m));
147          end
148          P_temp = mvnpdf(testpos(:,i)', testpos(:,j)' , 2* kernelwidth *eye(2) ) ;
149          for l=1:nr_targets
150             sigma_s(i,j) = sigma_s(i,j)+ P_temp * Pij(i,j,l) ;
151          end
152          sigma_s(i,j) = sigma_s(i,j)-   mu_s(i)*mu_s(j) ;
153      end
154  end
155 
156 %%% Step 3d: Calculate cross-covariance of predicted pseudo-measurement
157 sigma_xs = zeros(x_dim*nr_targets, size(testpos,2) );
158 
159 %%% Calculate Kalman gains
160 for  l =1:nr_targets
161  K{l} =  xCov(:,gti(l))*H*(H*xCov(gti(l),gti(l))*H'+measNoise+kernelwidth*eye(2))^-1;
162 end
163 
164 for i= 1:size(testpos,2) 
165    for l =1:nr_targets
166     sigma_xs(:,i) = sigma_xs(:,i)+  P(i,l)   *   (xEst + K{l} * (testpos(:,i)-H *xEst(gti(l))    )  );
167   end
168   sigma_xs(:,i) = sigma_xs(:,i)-   mu_s(i)*xEst;
169 end;
170 end
171 
172 
173 function w = mvnpdf(x,m,S)
174 %  Evaluates a Gaussian with mean m and covariance matrix S at location x.
175 %  Adapted from gauss_eval by Tim Bailey
176 %  taken from http://www-personal.acfr.usyd.edu.au/tbailey/software/matlab_utilities.htm
177 v = (x-m)';
178 D = size(v,1);
179 Sc = chol(S)';
180 nin = Sc\v; 
181 E = -0.5 * sum(nin.*nin, 1); 
182 C = (2*pi)^(D/2) * prod(diag(Sc)); 
183 w = exp(E) / C; 
184 end
185 
186 
187 function setAxisProps(tit)
188 hold on    
189 xlim([-4 10]);
190 ylim([-4 10]);
191 xlabel('x');
192 ylabel('y');
193 set(gca,'FontSize',8);
194 axis square;
195 title(tit);
196 end
Download algorithm (1 file) as ZIP

Comments

Please login to post a comment.