Error solution in particle filter
2 views (last 30 days)
Show older comments
I try to run simple particle filter but error keep appear saying unable to perform assignment because the size of the left side is 10-by-1 and the size of the right side is10-by-10 and error in line 43 saying x(:,k) = sys(k, x(:,k-1), u(:,k)); % simulate state
%% 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 = 10; % 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) 100 + vk; % (returns column vector)
%% PDF of process noise and noise generator function
nu = 10; % 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) [95;96;97;98;94;93;97;99;98;100]+ones(10,1)*normrnd(0, sqrt(0.0000001)); % 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 = 100;
%% Separate memory space
x = zeros(nx,T); y = zeros(ny,T);
u = zeros(nu,T); v = zeros(nv,T);
%% Simulate system
xh0 = 0; % 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) = 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 = 100; % 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
%% plot of the state vs estimated state by the particle filter vs particle paths
figure
hold on;
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(1) h3(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;
0 Comments
Answers (1)
Walter Roberson
on 17 Nov 2022
%% Process equation x[k] = sys(k, x[k-1], u[k]);% untuk state vector
nx = 10; % 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) 100 + vk; % (returns column vector)
%% PDF of process noise and noise generator function
nu = 10; % 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)
Notice that gen_sys_noise accepts a parameter but completely ignores it -- not, for example, using it to determine the size of the desired output.
%% 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)
Likewise gen_obs_noise ignores its parameter.
%% Initial PDF
% p_x0 = @(x) normpdf(x, 0,sqrt(10)); % initial pdf
gen_x0 = @(x) [95;96;97;98;94;93;97;99;98;100]+ones(10,1)*normrnd(0, sqrt(0.0000001)); % 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 = 100;
%% Separate memory space
x = zeros(nx,T); y = zeros(ny,T);
u = zeros(nu,T); v = zeros(nv,T);
%% Simulate system
xh0 = 0; % 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_temp = gen_sys_noise(); % simulate process noise
v_temp = gen_obs_noise(); % simulate observation noise
x_temp = sys(k, x(:,k-1), u(:,k)); % simulate state
y_temp = obs(k, x(:,k), v(:,k)); % simulate observation
whos u v x y u_temp v_temp x_temp y_temp
disp('about to u_temp')
u(:,k) = u_temp;
disp('about to v_temp')
v(:,k) = v_temp;
disp('about to x_temp')
x(:,k) = x_temp;
disp('about to y_temp')
y(:,k) = y_temp;
disp('got through temp')
end
the gen_*_noise functions each return a scalar. You assign the scalar to a (:,k) of an array, and that is fine as far as MATLAB cares: it copies the scalar to each of the output locations. Whether that makes sense mathematically for your case is a different matter -- should you perhaps be using a different noise value for each row?
You are calling sys() with column vectors for your second and third parameter. I explained in detail to you in https://www.mathworks.com/matlabcentral/answers/1848368-meaning-of-sys-k-xkm1-uk-xkm1-2-25-xkm1-1-xkm1-2-8-cos-1-2-k-uk#answer_1096918 that the / operator is the matrix-right-divide operation
"A matrix matrix-right-divide another matrix of the same size is well defined, and will be roughly the same as (25*xkm1) * pinv(1+xkm1^2) where pinv is pseudo-inverse and * is the inner-product operation. You will get out a matrix the same size as xkm1 ... provided that xkm1 was a square matrix"
but your xkm1 is not a square matrix. When you divide a row vector by another row vector the same size, the result is a square matrix that has as many rows and columns as there are rows in the row vectors.
What you should have understood from my long explaination is that you should not be using the MATLAB / operator, except possibly in cases where the right hand side is a scalar. A/2 is not going to cause you problems, but A/B where B is non-scalar is seldom what you want to do.
Only use A/B for cases where B is guaranteed scalar, or where you mean something algebraically equivalent to innerproduct between A and matrix inverse of B.
See Also
Categories
Find more on Polynomials in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!