- #1
viola83
- 1
- 0
Hi everyone, I am trying to implement in Matlab a particle filter to perform the orbit determination of a spacecraft .
I think my problem is the way I do the particle resampling. As you can see in the code I already tried to:
- scale the measurement difference (true - predicted) in order to preventing the exponential to go to zero (but maybe it is bad scaled?)
- try to use roughening in order to prevent sample impoverishment (anyway the code doesn't work event wothout this part)
does anybody can help?? :(
thank you very very much in advance...
Here is my code:
-----------------------------------------------------
% Simulation parameters
odereltol=2.22045e-14; odeabstol=2.22045e-14;
options = odeset('reltol', odereltol, 'abstol', odeabstol);
% Conversion Constants
l = 1.5e7; %[km]
tau = 24*3600*1e3; %
mu = 1.32712440018e11; %[km^3/s^2]
mu = mu*(tau^2)/(l^3);
% SPACECRAFT initial orbit:
r0 = [1.5e8; 2.814e7; 9.444e5]; % [ km ] initial position
v0 = [-4.5653; 29.28; 0.9824]; % [ km/s ] initial velocity
% Initial conditions in nondimensional units
r0 = r0/l;
v0 = v0*tau/l;
x0 = [r0; v0];
% Number of variables
nvar = 6;
npol = 6;
nvarmeas = 6;
nmeas = 3;
% Number of samples
Ns = 1e3;%1e4;
% Initial error covariance matrix
SigPos = (1e14)/(l^2); %[l]
SigVel = (1)*((tau/l)^2); %[l/tau]
P0 = [SigPos 0 0 0 0 0; 0 SigPos 0 0 0 0; 0 0 SigPos 0 0 0;...
0 0 0 SigVel 0 0; 0 0 0 0 SigVel 0; 0 0 0 0 0 SigVel];
% Measurement noise covariance matrix
R = zeros(nmeas,nmeas);
R(1,1) = (1e-3/l)^2;
R(2,2) = (1e-3/l)^2;
R(3,3) = (1e-3/l)^2;
% Simulation times
TOF = 0.5;
timestep = 4e-3;
tspan = [0:timestep:TOF];
Nt = length(tspan)
% Initialization
ytrue = zeros(length(tspan),nvar); % True state
x = zeros(nvar,Nt,Ns); % Particles
x_new = x; % Particles after resampling
particles = zeros(nvar,Ns);
xave = zeros(Nt,nvar); % Estimated mean
xave(1,:) = x0 + sqrt(P0)*randn(nvar,1);
w = zeros(Nt,Ns); % Weigths
w(1,:) = 1/Ns; % Initial weigts distribution
wn = zeros(Nt,Ns); % Normalized weights
ztrue = zeros(nmeas,Nt); % True measurement
zpred = zeros(nmeas,Nt,Ns); % Predicted measurement
zdiff = zeros(nmeas,Nt,Ns); % Measurement delta
zdiffscaled = zeros(nmeas,Nt); % Scaled measurement difference
%% TRUE MOTION
% Computation of the TRUE TRAJECTORY
ytrue(1,:) = x0;
k = 1;
for t = 0 : timestep : TOF-timestep
[tout,yout] = ode87(@TwoBodyProblem,[0 timestep],ytrue(k,:),options);
ytrue(k+1,:) = yout(end,:);
k = k+1;
end
% Computation of the TRUE MEASUREMENTS
k = 1;
for t = 0 : timestep : TOF
ztrue(1,k) = ytrue(k,1) + randn*sqrt(R(1,1));
ztrue(2,k) = ytrue(k,2) + randn*sqrt(R(2,2));
ztrue(3,k) = ytrue(k,3) + randn*sqrt(R(3,3));
k = k+1;
end
% Particles initialization
for i = 1:Ns
x(:,1,i) = xave(1,:)' + sqrt(P0)*randn(nvar,1);
end
%% FILTER
for k = 2 : Nt
---> here there is the part of the code that propagates the trajectory of each particle
finding x(:,k,i)
% Measurement prediction
zpred(1,k,i) = x(1,k,i);
zpred(2,k,i) = x(2,k,i);
zpred(3,k,i) = x(3,k,i);
% Scaled measurement difference
zdiff(:,k,i) = ztrue(:,k) - zpred(:,k,i);
end
for kk = 1:nmeas
zdiffscaled(kk,k) = max(abs(zdiff(kk,k,)/(1);
end
for i = 1 : Ns
w(k,i) = exp(-(zdiff(:,k,i)./zdiffscaled(:,k))'*(zdiff(:,k,i)./zdiffscaled(:,k)));
end
w(k,:)
% Normalize important weights
w(k,:) = w(k,:)/sum(w(k,;
% Selection by resampling
cumulative_sum_weights = cumsum(w(k,;
for jj=1:Ns
indx = min(find(cumulative_sum_weights>rand));
if(~isempty(indx))
x_new(:,k,jj) = x(:,k,indx);
else
x_new(:,k,jj) = x(:,k,jj);
end
% Use roughening to prevent sample impoverishment
particles(:,:) = x(:,k,:);
E = max(particles')' - min(particles')';
sigma = 0.2 * E * Ns^(-1/nvar);
x_new(:,k,jj) = x_new(:,k,jj) + sigma .* randn(6,1);
end
for jj = 1:Ns
w(k,jj) = 1/Ns;
end
% Estimated mean computation
for i = 1:Ns
xave(k,:) = xave(k,:) + x(:,k,i)'/Ns;
end
end
---------------------------------------------------------------------------------
I think my problem is the way I do the particle resampling. As you can see in the code I already tried to:
- scale the measurement difference (true - predicted) in order to preventing the exponential to go to zero (but maybe it is bad scaled?)
- try to use roughening in order to prevent sample impoverishment (anyway the code doesn't work event wothout this part)
does anybody can help?? :(
thank you very very much in advance...
Here is my code:
-----------------------------------------------------
% Simulation parameters
odereltol=2.22045e-14; odeabstol=2.22045e-14;
options = odeset('reltol', odereltol, 'abstol', odeabstol);
% Conversion Constants
l = 1.5e7; %[km]
tau = 24*3600*1e3; %
mu = 1.32712440018e11; %[km^3/s^2]
mu = mu*(tau^2)/(l^3);
% SPACECRAFT initial orbit:
r0 = [1.5e8; 2.814e7; 9.444e5]; % [ km ] initial position
v0 = [-4.5653; 29.28; 0.9824]; % [ km/s ] initial velocity
% Initial conditions in nondimensional units
r0 = r0/l;
v0 = v0*tau/l;
x0 = [r0; v0];
% Number of variables
nvar = 6;
npol = 6;
nvarmeas = 6;
nmeas = 3;
% Number of samples
Ns = 1e3;%1e4;
% Initial error covariance matrix
SigPos = (1e14)/(l^2); %[l]
SigVel = (1)*((tau/l)^2); %[l/tau]
P0 = [SigPos 0 0 0 0 0; 0 SigPos 0 0 0 0; 0 0 SigPos 0 0 0;...
0 0 0 SigVel 0 0; 0 0 0 0 SigVel 0; 0 0 0 0 0 SigVel];
% Measurement noise covariance matrix
R = zeros(nmeas,nmeas);
R(1,1) = (1e-3/l)^2;
R(2,2) = (1e-3/l)^2;
R(3,3) = (1e-3/l)^2;
% Simulation times
TOF = 0.5;
timestep = 4e-3;
tspan = [0:timestep:TOF];
Nt = length(tspan)
% Initialization
ytrue = zeros(length(tspan),nvar); % True state
x = zeros(nvar,Nt,Ns); % Particles
x_new = x; % Particles after resampling
particles = zeros(nvar,Ns);
xave = zeros(Nt,nvar); % Estimated mean
xave(1,:) = x0 + sqrt(P0)*randn(nvar,1);
w = zeros(Nt,Ns); % Weigths
w(1,:) = 1/Ns; % Initial weigts distribution
wn = zeros(Nt,Ns); % Normalized weights
ztrue = zeros(nmeas,Nt); % True measurement
zpred = zeros(nmeas,Nt,Ns); % Predicted measurement
zdiff = zeros(nmeas,Nt,Ns); % Measurement delta
zdiffscaled = zeros(nmeas,Nt); % Scaled measurement difference
%% TRUE MOTION
% Computation of the TRUE TRAJECTORY
ytrue(1,:) = x0;
k = 1;
for t = 0 : timestep : TOF-timestep
[tout,yout] = ode87(@TwoBodyProblem,[0 timestep],ytrue(k,:),options);
ytrue(k+1,:) = yout(end,:);
k = k+1;
end
% Computation of the TRUE MEASUREMENTS
k = 1;
for t = 0 : timestep : TOF
ztrue(1,k) = ytrue(k,1) + randn*sqrt(R(1,1));
ztrue(2,k) = ytrue(k,2) + randn*sqrt(R(2,2));
ztrue(3,k) = ytrue(k,3) + randn*sqrt(R(3,3));
k = k+1;
end
% Particles initialization
for i = 1:Ns
x(:,1,i) = xave(1,:)' + sqrt(P0)*randn(nvar,1);
end
%% FILTER
for k = 2 : Nt
---> here there is the part of the code that propagates the trajectory of each particle
finding x(:,k,i)
% Measurement prediction
zpred(1,k,i) = x(1,k,i);
zpred(2,k,i) = x(2,k,i);
zpred(3,k,i) = x(3,k,i);
% Scaled measurement difference
zdiff(:,k,i) = ztrue(:,k) - zpred(:,k,i);
end
for kk = 1:nmeas
zdiffscaled(kk,k) = max(abs(zdiff(kk,k,)/(1);
end
for i = 1 : Ns
w(k,i) = exp(-(zdiff(:,k,i)./zdiffscaled(:,k))'*(zdiff(:,k,i)./zdiffscaled(:,k)));
end
w(k,:)
% Normalize important weights
w(k,:) = w(k,:)/sum(w(k,;
% Selection by resampling
cumulative_sum_weights = cumsum(w(k,;
for jj=1:Ns
indx = min(find(cumulative_sum_weights>rand));
if(~isempty(indx))
x_new(:,k,jj) = x(:,k,indx);
else
x_new(:,k,jj) = x(:,k,jj);
end
% Use roughening to prevent sample impoverishment
particles(:,:) = x(:,k,:);
E = max(particles')' - min(particles')';
sigma = 0.2 * E * Ns^(-1/nvar);
x_new(:,k,jj) = x_new(:,k,jj) + sigma .* randn(6,1);
end
for jj = 1:Ns
w(k,jj) = 1/Ns;
end
% Estimated mean computation
for i = 1:Ns
xave(k,:) = xave(k,:) + x(:,k,i)'/Ns;
end
end
---------------------------------------------------------------------------------