Particle filter process equation

Hi, I try to run the particle filter coding, and I realize that I only can get answer when I run it with the process equation

sys=@(k, xkm1, uk) xkm1/2 + 25*xkm1/(1+xkm1^2) + 8*cos(1.2*k) + uk; % (returns column vector) in line 5,

and I realize that the graph that I get is around 0.

I plan to change tha process equation to @(k, xkm1, uk) xkm1/2 + 25*xkm1/(1+xkm1^2) + 8*cos(1.2*k) + uk; % (returns column vector) but error keep coming out. Is there any recommendation that I can do in order to use this process equation? Really appreciate your help.

The code is,

%% clear memory, screen, and close all figures
clear, clc, close all;
%% Process equation x[k] = sys(k, x[k-1], u[k]);% untuk state vector
nx = 50;  % number of states
sys=@(k, xkm1, uk) xkm1/2 + 25*xkm1/(1+xkm1^2) + 8*cos(1.2*k) + uk; % (returns column vector)

%% Observation equation y[k] = obs(k, x[k], v[k]);
ny = 1;  % number of observations
xk=100;
obs =  @(k, xk,vk) xk(1) + vk;                   % (returns column vector)
%% PDF of process noise and noise generator function
nu = 50;                                           % size of the vector of process noise
sigma_u = sqrt(10);
p_sys_noise   = @(u) normpdf(u, 0, sigma_u);
gen_sys_noise = @(u) normrnd(0, sigma_u);         % sample from p_sys_noise (returns column vector)
%% PDF of observation noise and noise generator function
nv = 1;                                           % size of the vector of observation noise
sigma_v = sqrt(10);
p_obs_noise   = @(v) normpdf(v, 0, sigma_v);
gen_obs_noise = @(v) normrnd(0, sigma_v);         % sample from p_obs_noise (returns column vector)
%% Initial PDF
% p_x0 = @(x) normpdf(x, 0,sqrt(10));             % initial pdf
gen_x0 = @(x) [89.4;93.75;90;94.38;98.75;94.38;96.25;94.38;96.25;100;95;96.25;95;95;98.75;98.125;97.5;98.75;100;100;91.875;93.75;93.125;96.25;98.125;98.125;99.375;100;100;100;93.75;92.5;96.25;95.625;99.375;97.5;100;99.375;99.375;100;95.625;95.625;97.5;97.5;98.75;99.375;99.375;91.25;98.125;100]+ones(50,1)*normrnd(0, sqrt(10));     % sample from p_x0 (returns column vector)
%% Transition prior PDF p(x[k] | x[k-1])
% (under the suposition of additive process noise)
% p_xk_given_xkm1 = @(k, xk, xkm1) p_sys_noise(xk - sys(k, xkm1, 0));
%% Observation likelihood PDF p(y[k] | x[k])
% (under the suposition of additive process noise)
p_yk_given_xk = @(k, yk, xk) p_obs_noise((yk - obs(k, xk, zeros(1, nv)))');
%% %% Number of time steps
T = 100;
%% Separate memory space
x = zeros(nx,T);  y = zeros(ny,T);
u = zeros(nu,T);  v = zeros(nv,T);
%% Simulate system
xh0 = [89.4;93.75;90;94.38;98.75;94.38;96.25;94.38;96.25;100;95;96.25;95;95;98.75;98.125;97.5;98.75;100;100;91.875;93.75;93.125;96.25;98.125;98.125;99.375;100;100;100;93.75;92.5;96.25;95.625;99.375;97.5;100;99.375;99.375;100;95.625;95.625;97.5;97.5;98.75;99.375;99.375;91.25;98.125;100];                                   % initial state
u(:,1) = 0;                               % initial process noise
v(:,1) = gen_obs_noise(sigma_v);          % initial observation noise
x(:,1) = xh0;
y(:,1) = obs(1, xh0, v(:,1));
for k = 2:T
    % here we are basically sampling from p_xk_given_xkm1 and from p_yk_given_xk
   u(:,k) = 0.1*gen_sys_noise();              % simulate process noise
   v(:,k) = gen_obs_noise();              % simulate observation noise
   x(:,k) = sys(k, x(:,k-1), u(:,k));     % simulate state
   y(:,k) = obs(k, x(:,k),   v(:,k));     % simulate observation
end

fprintf('Finish simulate system \n')
%% Separate memory
xh = zeros(nx, T); xh(:,1) = xh0;
xh_ukf = zeros(nx, T); xh_ukf(:,1) = xh0;
yh = zeros(ny, T); yh(:,1) = obs(1, xh0, zeros(1, nv));

pf.k               = 1;                   % initial iteration number
pf.Ns              = 50;                 % number of particles
pf.w               = zeros(pf.Ns, T);     % weights
pf.particles       = zeros(nx, pf.Ns, T); % particles
pf.gen_x0          = gen_x0;              % function for sampling from initial pdf p_x0
pf.p_yk_given_xk   = p_yk_given_xk;       % function of the observation likelihood PDF p(y[k] | x[k])
pf.gen_sys_noise   = gen_sys_noise;       % function for generating system noise
%pf.p_x0 = p_x0;                          % initial prior PDF p(x[0])
%pf.p_xk_given_ xkm1 = p_xk_given_xkm1;   % transition prior PDF p(x[k] | x[k-1])


%% Estimate state

 for k = 2:T
   fprintf('Iteration = %d/%d\n',k,T);
   % state estimation
   pf.k = k;
   %[xh(:,k), pf] = particle_filter(sys, y(:,k), pf, 'multinomial_resampling');
   [xh(:,k), pf] = particle_filter(sys, y(:,k), pf, 'systematic_resampling');   

   
   % filtered observation
   yh(:,k) = obs(k, xh(:,k), zeros(1, nv));
   %hold on
   %plot(my_pf.pf.w, my_pf.pf.particles(1,:,k),'k.')
end
%% Compute RMSE
err = (xh - x).*(xh - x);
RMSE_pf = sqrt(sum(err,2)/T);

err = (xh_ukf - x).*(xh_ukf - x);
RMSE_ukf = sqrt(sum(err,2)/T);

%%  Make plots of the evolution of the density
figure
hold on;
xi = 1:T;
yi = -25:0.25:25;
[xx,yy] = meshgrid(xi,yi);
den = zeros(size(xx));
xhmode = zeros(size(xh));
for i = xi
   % for each time step perform a kernel density estimation
   den(:,i) = ksdensity(pf.particles(1,:,i), yi,'kernel','epanechnikov');
   [~, idx] = max(den(:,i));
   % estimate the mode of the density
   xhmode(i) = yi(idx);
   plot3(repmat(xi(i),length(yi),1), yi', den(:,i));
end
view(3);
box on;
title('Evolution of the state density','FontSize',14)
figure
mesh(xx,yy,den);   
title('Evolution of the state density','FontSize',14)

%% plot of the state vs estimated state by the particle filter vs particle paths
figure
hold on;

%h1 = plot(1:T,squeeze(pf.particles),'y');
h1 = plot(1:T,squeeze(pf.particles(2,:,:)),'y');
h2 = plot(1:T,x(1,:),'b','LineWidth',1);
h3 = plot(1:T,xh(1,:),'r','LineWidth',1);
h4 = plot(1:T,y(1,:),'g.','LineWidth',1);
legend([h2 h3 h1(1)],'state','mean of estimated state','particle paths');
title('State vs estimated state by the particle filter vs particle paths','FontSize',14);

%%  plot of the observation vs filtered observation by the particle filter
figure
plot(1:T,y,'b', 1:T,yh,'r');
legend('observation','filtered observation');
title('Observation vs filtered observation by the particle filter','FontSize',14);

return;

Answer 1

[Raag]

Hi,

As per my understanding, the issue you are encountering is related to the use of matrix operations “(/, ^)” instead of element-wise operations “(./, .^)” in your process equation. In your particle filter implementation, the state variable xkm1 is a vector (for example, of size 50x1), so all operations involving xkm1 should be performed elementwise to avoid dimension mismatch errors.

You can resolve this by updating your process equation to use element-wise operators as shown below:

sys = @(k, xkm1, uk) xkm1/2 + 25xkm1./(1 + xkm1.^2) + 8cos(1.2*k) + uk;

Here, ./ and .^ ensure that the division and exponentiation are performed element-wise across the vector xkm1.

Using / and ^ performs matrix division and matrix power, which are not valid for vectors in this context so by switching to ./ and .^, MATLAB applies the operation to each element of the vector individually, which is necessary for your particle filter simulation to work correctly.

For a better understanding of the above solution, refer to the following MATLAB documentations:

1. Array-vs matrix operation: https://www.mathworks.com/help/matlab/matlab_prog/array-vs-matrix-operations.html

2. anonymous functions: https://www.mathworks.com/help/matlab/matlab_prog/anonymous-functions.html