Robustify a particle filter - particle-filter

i would like to have your thought on the following code extract.
I have the uniform probability density function:
p(x) = 1/S if -S/2<x<S/2
p(x) = 0 else
My goal is to avoid having the weight becoming 0, and for that I need to robustify the likelihood calculation so that it does not go to 0.
For that I know I need to modify the if condition, but I am not sure what I should put instead.
% Particles weight update
S=20;
for i=1:N
for j=1:P_beacon
% Innovation
mu=abs(norm(particle(i).x(1:2)-P_beacon(:,j))-y(j,k));
if mu>= S/2
likelihood=0;
else
likelihood=1/S;
end
particle(i).w=particle(i).w*prod(likelihood);
end
end

Related

how to replace the ode45 method with the runge-kutta in this matlab?

I tried everything and looked everywhere but can't find any solution for my question.
clc
clear all
%% Solving the Ordinary Differential Equation
G = 6.67408e-11; %Gravitational constant
M = 10; %Mass of the fixed object
r = 1; %Distance between the objects
tspan = [0 100000]; %Time Progression from 0 to 100000s
conditions = [1;0]; %y0= 1m apart, v0=0 m/s
F=#(t,y)var_r(y,G,M,r);
[t,y]=ode45(F,tspan,conditions); %ODE solver algorithm
%%part1: Plotting the Graph
% plot(t,y(:,1)); %Plotting the Graph
% xlabel('time (s)')
% ylabel('distance (m)')
%% part2: Animation of Results
plot(0,0,'b.','MarkerSize', 40);
hold on %to keep the first graph
for i=1:length(t)
k = plot(y(i,1),0,'r.','MarkerSize', 12);
pause(0.05);
axis([-1 2 -2 2]) %Defining the Axis
xlabel('X-axis') %X-Axis Label
ylabel('Y-axis') %Y-Axis Label
delete(k)
end
function yd=var_r(y,G,M,r) %function of variable r
g = (G*M)/(r + y(1))^2;
yd = [y(2); -g];
end
this is the code where I'm trying to replace the ode45 with the runge kutta method but its giving me errors. my runge kutta function:
function y = Runge_Kutta(f,x0,xf,y0,h)
n= (xf-x0)/h;
y=zeros(n+1,1);
x=(x0:h:xf);
y(1) = y0;
for i=1:n
k1 = f(x(i),y(i));
k2= f(x(i)+ h/2 , y(i) +h*(k1)/2);
y(i+1) = y(i)+(h*k2);
end
plot(x,y,'-.M')
legend('RKM')
title ('solution of y(x)');
xlabel('x');
ylabel('y(x)')
hold on
end
Before converting your ode45( ) solution to manually written RK scheme, it doesn't even look like your ode45( ) solution is correct. It appears you have a gravitational problem set up where the initial velocity is 0 so a small object will simply fall into a large mass M on a line (rectilinear motion), and that is why you have scalar position and velocity.
Going with this assumption, r is something you should be calculating on the fly, not using as a fixed input to the derivative function. E.g., I would have expected something like this:
F=#(t,y)var_r(y,G,M); % get rid of r
:
function yd=var_r(y,G,M) % function of current position y(1) and velocity y(2)
g = (G*M)/y(1)^2; % gravity accel based on current position
yd = [y(2); -g]; % assumes y(1) is positive, so acceleration is negative
end
The small object must start with a positive initial position for the derivative code to be valid as you have it written. As the small object falls into the large mass M, the above will only hold until it hits the surface or atmosphere of M. Or if you model M as a point mass, then this scheme will become increasingly difficult to integrate correctly because the acceleration becomes large without bound as the small mass gets very close to the point mass M. You would definitely need a variable step size approach in this case. The solution becomes invalid if it goes "through" mass M. In fact, once the speed gets too large the whole setup becomes invalid because of relativistic effects.
Maybe you could explain in more detail if your system is supposed to be set up this way, and what the purpose of the integration is. If it is really supposed to be a 2D or 3D problem, then more states need to be added.
For your manual Runge-Kutta code, you completely forgot to integrate the velocity so this is going to fail miserably. You need to carry a 2-element state from step to step, not a scalar as you are currently doing. E.g., something like this:
y=zeros(2,n+1); % 2-element state as columns of the y variable
x=(x0:h:xf);
y(:,1) = y0; % initial state is the first 2-element column
% change all the scalar y(i) to column y(:,i)
for i=1:n
k1 = f(x(i),y(:,i));
k2= f(x(i)+ h/2 , y(:,i) +h*(k1)/2);
y(:,i+1) = y(:,i)+(h*k2);
end
plot(x,y(1,:),'-.M') % plot the position part of the solution
This is all assuming the f that gets passed in is the same F you have in your original code.
y(1) is the first scalar element in the data structure of y (this counts in column-first order). You want to generate in y a list of column vectors, as your ODE is a system with state dimension 2. Thus you need to generate y with that format, y=zeros(length(x0),n+1); and then address the list entries as matrix columns y(:,1)=x0 and the same modification in every place where you extract or assign a list entry.
Matlab introduce various short-cuts that, if used consequently, lead to contradictions (I think the script-hater rant (german) is still valid in large parts). Essentially, unlike in other systems, Matlab gives direct access to the underlying data structure of matrices. y(k) is the element of the underlying flat array (that is interpreted column-first in Matlab like in Fortran, unlike, e.g., Numpy where it is row-first).
Only the two-index access is to the matrix with its dimensions. So y(:,k) is the k-th matrix column and y(k,:) the k-th matrix row. The single-index access is nice for row or column vectors, but leads immediately to problems when collecting such vectors in lists, as these lists are automatically matrices.

Statistical test gives result different than reality?

Let's define X as :
and objects releated to it:
I want to calculation value of function following, and plot it on the same graph with histogram eigenvalues of Y.
After that I want to perform chi2gof test to judge weather those two distributions converge to each other or not. I want to use parameter expected with is designed to compare distributions of function and histogram.
My work so far
clf;
m=8000;
n=10000;
X=randn(m,n);
Y=X*X'/n;
sd=std(X(:));
l=m/n;
eigs=eig(Y);
lp=sd^2*(1+sqrt(l))^2;
lm=sd^2*(1-sqrt(l))^2;
x=linspace(0.00000001,lp,n);
for i = 1:length(x)
if (and(x(i) <= lp, x(i) >= lm))
dv(i) = sqrt((lp-x(i)).*(x(i)-lm))./(2*pi*sd^2*l.*x(i));
else
dv(i) = 0;
end
end
This code fully calculates my function dv. Now to plot it on histogram, and to add sens to it I normalized histogram to have unit area.
hold on;
[h, centres] = hist(eigs, 50);
% normalise to unit area
norm_h = h / (numel(eigs) * (centres(2)-centres(1)));
bar(centres, norm_h);
plot(x, dv, "r");
hold off;
The result from this code is image following:
As we can see the dv line really nicely fits the histogram. We can be almost sure that chi square test for same distribution should output p value very close to 1 (it means that samples are from the same distribution). However code
[h,p,stats] = chi2gof(dv,'Expected',norm_h);
outputs
p =
0
It means that null hypothesis of the same distributions were rejected. My question is - how ? Am I using something incorrectly, or this pvalue is really 0 ?

numerical integration for Gaussian function - indefinite integral

My approach
fun = #(y) (1/sqrt(pi))*exp(-(y-1).^2).*log(1 + exp(-4*y))
integral(fun,-Inf,Inf)
This gives NaN.
So I tried plotting it.
y= -10:0.1:10;
plot(y,exp(-(y-1).^2).*log(1 + exp(-4*y)))
Then understood that domain (siginificant part) is from -4 to +4.
So changed the limits to
integral(fun,-10,10)
However I do not want to always plot the graph and then know its limits. So is there any way to know the integral directly from -Inf to Inf.
Discussion
If your integrals are always of the form
I would use a high-order Gauss–Hermite quadrature rule.
It's similar to the Gauss-Legendre-Kronrod rule that forms the basis for quadgk but is specifically tailored for integrals over the real line with a standard Gaussian multiplier.
Rewriting your equation with the substitution x = y-1, we get
.
The integral can then be computed using the Gauss-Hermite rule of arbitrary order (within reason):
>> order = 10;
>> [nodes,weights] = GaussHermiteRule(order);
>> f = #(x) log(1 + exp(-4*(x+1)))/sqrt(pi);
>> sum(f(nodes).*weights)
ans =
0.1933
I'd note that the function below builds a full order x order matrix to compute nodes, so it shouldn't be made too large.
There is a way to avoid this by explicitly computing the weights, but I decided to be lazy.
Besides, event at order 100, the Gaussian multiplier is about 2E-98, so the integrand's contribution is extremely minimal.
And while this isn't inherently adaptive, a high-order rule should be sufficient in most cases ... I hope.
Code
function [nodes,weights] = GaussHermiteRule(n)
% ------------------------------------------------------------------------------
% Find the nodes and weights for a Gauss-Hermite Quadrature integration.
%
if (n < 1)
error('There is no Gauss-Hermite rule of order 0.');
elseif (n < 0) || (abs(n - round(n)) > eps())
error('Given order ''n'' must be a strictly positive integer.');
else
n = round(n);
end
% Get the nodes and weights from the Golub-Welsch function
n = (0:n)' ;
b = n*0 ;
a = b + 0.5 ;
c = n ;
[nodes,weights] = GolubWelsch(a,b,c,sqrt(pi));
end
function [xk,wk] = GolubWelsch(ak,bk,ck,mu0)
%GolubWelsch
% Calculate the approximate* nodes and weights (normalized to 1) of an orthogonal
% polynomial family defined by a three-term reccurence relation of the form
% x pk(x) = ak pkp1(x) + bk pk(x) + ck pkm1(x)
%
% The weight scale factor mu0 is the integral of the weight function over the
% orthogonal domain.
%
% Calculate the terms for the orthonormal version of the polynomials
alpha = sqrt(ak(1:end-1) .* ck(2:end));
% Build the symmetric tridiagonal matrix
T = full(spdiags([[alpha;0],bk,[0;alpha]],[-1,0,+1],length(alpha),length(alpha)));
% Calculate the eigenvectors and values of the matrix
[V,xk] = eig(T,'vector');
% Calculate the weights from the eigenvectors - technically, Golub-Welsch requires
% a normalization, but since MATLAB returns unit eigenvectors, it is omitted.
wk = mu0*(V(1,:).^2)';
end
I've had success with transforming such infinite-bounded integrals using a numerical variable transformation, as explained in Numerical Recipes 3e, section 4.5.3. Basically, you substitute in y=c*tan(t)+b and then numerically integrate over t in (-pi/2,pi/2), which sweeps y from -infinity to infinity. You can tune the values of c and b to optimize the process. This approach largely dodges the question of trying to determine cutoffs in the domain, but for this to work reliably using quadrature you have to know that the integrand does not have features far from y=b.
A quick and dirty solution would be to look for a position, where your function is sufficiently small enough and then taking it as limits. This assumes that for x>0 the function fun decreases montonically and fun(x) is roughly the same size as fun(-x) for all x.
%// A small number
epsilon = eps;
%// Stepsize for searching bound
stepTest = 1;
%// Starting position for searching bound
position = 0;
%// Not yet small enough
smallEnough = false;
%// Search bound
while ~smallEnough
smallEnough = (fun(position) < eps);
position = position + stepTest;
end
%// Calculate integral
integral(fun, -position, position)
If your were happy with plotting the function, deciding by eye where you can cut, then this code will suffice, I guess.

MATLAB weighted resampling

I'm writing a particle filter localization algorithm as part of an exercise to locate a plane flying over mountains.
From my understanding, the steps to this are:
- make a bunch of random guesses
- filter out unlikely guesses (using Gaussian hypothesis testing and some known information about the problem)
- shift filtered points by how much the plane moved in that step
- resample, weighted by shifted points
What I'm having trouble with is the resampling bit - how could I perform a weighted resampling in MATLAB?
Please let me know if there's anything I should clarify!! Thanks!
Firstly you should look into the SIR (Sequential Importance Sampling Re-sampling) Particle Filter [PF] (Or Sequential Monte-Carlo Methods is the other name it is known by).
I recommend the book called by Arnaud Doucet & Neil Gordon called "Sequential Monte Carlo Methods in Practice". It contains practically the state of the art when it comes to Particle Filters and contains a description of the implementations of the various flavors of the PF.
The SIR-PF has the following steps:
Prediction: Based on your state equations and the previous particle population propagate the particles to the next discrete time instance i.e. x(t+1) = f(x(t),w(t)) := where x is a vector of n states and for each state you have N realisations (particles) of the state eg. x ~ [N x n]
Correction: based on your estimation of your measurement equations that should be in the form y(t+1) = g(x(t+1),v(t)), where x(t+1) is your state population of particles. You calculate the error, e(t) = y(t+1) - y_m(t+1) and weight the population according to a likelihood function, which can be, but not necessarily has to be, a Normal distribution. You now will have a set of weights e.g. if you have m "sensors" you will have a weighting matrix W = [N x m] or in the simple case you'll have a [N x 1] vector of weights. (remember to normalise the weights)
Re-sampling (Conditional): This step should be based on a conditional to avoid the pitfall of particle degeneracy (which you should look into), the common conditional is to compute the "effective particle population size", := 1/(sum of the squared weights) i.e. Neff = 1/sum(w1**2, w2**2, ...., wN**2). If Neff < 0.85*N then resample.
Re-sampling: Calculate the CDF of the (normalised) weights vector i.e. P = cumsum(W) and generate random samples from a uniform distribution (r), select the first particle that P(w) >= r, repeat this until you have N realisations of the CDF, this will sample more frequently from the particles that have higher weights and less frequently from those that do not, effectively condensing your particle population. You then create a new set of weights that are uniformly weighted i.e. wN = 1/N
function [weights,X_update] = Standardised_Resample(P,X)
Neff = 1/(sum(P.^2)); % Test effective particle size
P = P./sum(P) % Ensure particle weights are normalised
if Neff < 0.85*size(P,1)
N = size(P,1)
X_update(N,1) = 0
L = cumsum(P)
for i = 1:N
X_update(i) = X(find(rand <= L,1))
end
weights = ones(N,1)*1./N;
else
weights = P;
X_update = X;
end
end
Estimation: XEst = W(t+1)*x(t+1) := the weighted product produces the estimate for the states at time t+1
Rinse and Repeat for time t+2 etc.
Note: x(0/0) is a population of N samples of a random distribution of ~N(x(0),Q(0)) where x(0) is an estimate of the initial conditions [IC] and Q(0/0) is an estimate of the variance (uncertainty) of your IC guess

simulating a random walk in matlab

i have variable x that undergoes a random walk according to the following rules:
x(t+1)=x(t)-1; probability p=0.3
x(t+1)=x(t)-2; probability q=0.2
x(t+1)=x(t)+1; probability p=0.5
a) i have to create this variable initialized at zero and write a for loop for 100 steps and that runs 10000 times storing each final value in xfinal
b) i have to plot a probability distribution of xfinal (a histogram) choosing a bin size and normalization!!* i have to report the mean and variance of xfinal
c) i have to recreate the distribution by application of the central limit theorem and plot the probability distribution on the same plot!
help would be appreciated in telling me how to choose the bin size and normalize the histogram and how to attempt part c)
your help is much appreciated!!
p=0.3;
q=0.2;
s=0.5;
numberOfSteps = 100;
maxCount = 10000;
for count=1:maxCount
x=0;
for i = 1:numberOfSteps
random = rand(1, 1);
if random <=p
x=x-1;
elseif random<=(p+q)
x=x-2;
else
x=x+1;
end
end
xfinal(count) = x;
end
[f,x]=hist(xfinal,30);
figure(1)
bar(x,f/sum(f));
xlabel('xfinal')
ylabel('frequency')
mean = mean(xfinal)
variance = var(xfinal)
For the first question, check the help for hist on mathworks homepage
[nelements,centers] = hist(data,nbins);
You do not select the bin size, but the number of bins. nelements gives the elements per bin and center is all the bin centers. So to say, it would be the same to call
hist(data,nbins);
as
[nelements,centers] = hist(data,nbins);
plot(centers,nelements);
except that the representation is different (line or pile). To normalize, simply divide nelements with sum(nelements)
For c, here i.i.d. variables it actually is a difference if the variables are real or complex. However for real variables the central limit theorem in short tells you that for a large number of samples the distribution will limit the normal distribution. So if the samples are real, you simply asssumes a normal distribution, calculates the mean and variance and plots this as a normal distribution. If the variables are complex, then each of the variables will be normally distributed which means that you will have a rayleigh distribution instead.
Mathworks is deprecating hist that is being replaced with histogram.
more details in this link
You are not applying the PDF function as expected, the expression Y doesn't work
For instance Y does not have the right X-axis start stop points. And you are using x as input to Y while x already used as pivot inside the double for loop.
When I ran your code Y generates a single value, it is not a vector but just a scalar.
This
bar(x,f/sum(f));
bringing down all input values with sum(f) division? no need.
On attempting to overlap the ideal probability density function, often one has to do additional scaling, to have both real and ideal visually overlapped.
MATLAB can do the scaling for us, and no need to modify input data /sum(f).
With a dual plot using yyaxis
You also mixed variance and standard deviation.
Instead try something like this
y2=1 / sqrt(2*pi*var1)*exp(-(x2-m1).^2 / (2*var1))
ok, the following solves your question(s)
codehere
clear all;
close all;
clc
p=0.3; % thresholds
q=0.2;
s=0.5;
n_step=100;
max_cnt=10000;
n_bin=30; % histogram amount bins
xf=zeros(1,max_cnt);
for cnt=1:max_cnt % runs loop
x=0;
for i = 1:n_step % steps loop
t_rand1 = rand(1, 1);
if t_rand1 <=p
x=x-1;
elseif t_rand1<=(p+q)
x=x-2;
else
x=x+1;
end
end
xf(cnt) = x;
end
% [f,x]=hist(xf,n_bin);
hf1=figure(1)
ax1=gca
yyaxis left
hp1=histogram(xf,n_bin);
% bar(x,f/sum(f));
grid on
xlabel('xf')
ylabel('frequency')
m1 = mean(xf)
var1 = var(xf)
s1=var1^.5 % sigma
%applying central limit theorem %finding the mean
n_x2=1e3 % just enough points
min_x2=min(hp1.BinEdges)
max_x2=max(hp1.BinEdges)
% quite same as
min_x2=hp1.BinLimits(1)
max_x2=hp1.BinLimits(2)
x2=linspace(min_x2,max_x2,n_x2)
y2=1/sqrt(2*pi*var1)*exp(-(x2-m1).^2/(2*var1));
% hold(ax1,'on')
yyaxis right
plot(ax1,x2,y2,'r','LineWidth',2)
.
.
.
note I have not used these lines
% Xp=-1; Xq=-2; Xs=1; mu=Xp.*p+Xq.*q+Xs.*s;
% muN=n_step.*mu;
%
% sigma=(Xp).^2.*p+(Xq).^2.*q+(Xs).^2.s; % variance
% sigmaN=n_step.(sigma-(mu).^2);
People ususally call sigma to variance^.5
This supplied script is a good start point to now take it to wherever you need it to go.