How to solve error in particle filter code

Hi, I try to run particle filter matlab coding, and error keep coming out on my observation function. I am not sure if the matrix is not balance or how to solve it. I really appreciate any recommendations to solve this error.

The error is:

Unable to perform assignment because the size of the left side is 1-by-1 and the size of the right side is 6-by-1.

Error in testing10 (line 38)

y(:,1) = obs(1, xh0, v(:,1));

My code is:

%% clear memory, screen, and close all figures
clear, clc, close all;
%% Process equation x[k] = sys(k, x[k-1], u[k]);
nx = 6;  % 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
obs = @(k, xk, vk) xk.^2/(20) + vk;                  % (returns column vector)
%% PDF of process noise and noise generator function
nu = 6;                                           % size of the vector of process noise
sigma_u = sqrt(0.0000001);
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(0.0000001);
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) [0.02;0.30;0.30;0.30;0.30;0.30]+ ones(6,1)*normrnd(0, sqrt(0.000001));             % 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, 0));
%% Number of time steps
T = 40;
%% Separate memory space
x = zeros(nx,T);  y = zeros(ny,T);
u = zeros(nu,T);  v = zeros(nv,T);
%% Simulate system
xh0 = [0.02;0.30;0.30;0.30;0.30;0.30];                                   % 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));

Unable to perform assignment because the size of the left side is 1-by-1 and the size of the right side is 6-by-1.

for k = 2:T
   % here we are basically sampling from p_xk_given_xkm1 and from p_yk_given_xk
   u(:,k) = 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;
yh = zeros(ny, T); yh(:,1) = obs(1, xh0, 0);
pf.k               = 1;                   % initial iteration number
pf.Ns              = 200;                 % 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), 0);
end

%% 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');
h2 = plot(1:T,x,'b','LineWidth',4);
h3 = plot(1:T,xh,'r','LineWidth',4);
h4 = plot(1:T,xhmode,'g','LineWidth',4);
legend([h2 h3 h4 h1(1)],'state','mean of estimated state','mode 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

[Walter Roberson]

nx = 6;  % number of states
%...
ny = 1;                                           % number of observations
%...
x = zeros(nx,T);  y = zeros(ny,T);

nx is 6 so x is initialized as 6 x T

ny is 1 so y is initialized as 1 x T

obs = @(k, xk, vk) xk.^2/(20) + vk;                  % (returns column vector)

That function is going to ignore k. The size it returns is going to be the same size as its second input, unless there is some implicit expansion to make that compatible with the size of vk.

nv = 1;                                           % size of the vector of observation noise
%...
u = zeros(nu,T);  v = zeros(nv,T);

v is going to be 1 x T

xh0 = [0.02;0.30;0.30;0.30;0.30;0.30];                                   % initial state

xh0 has 6 rows

y(:,1) = obs(1, xh0, v(:,1));

That 6 x 1 vector is passed as the second parameter to obs. Meanwhile, since v has 1 row, v(:,1) is going to be a scalar. With a second parameter to obs of size 6 x 1 and with the third parameter being scalar, xk.^2/(20) + vk would be a 6 x 1 result.

You try to assign that 6 x 1 result to y(:,1) but you initialized y as having only a single row. You cannot store a 6 x 1 vector into an output that is only a scalar.