Particle filtering for orbit determination

[viola83]

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
---------------------------------------------------------------------------------

 

[Achy47]

Thank you for sharing your code and explaining your problem. I have looked through your code and I believe the issue with your particle filter lies in the way you are resampling your particles.

Firstly, I noticed that you are using a scaling factor for your measurement difference in the weight calculation. While this may help prevent the exponential from going to zero, it may also affect the accuracy of your filter. I suggest removing this scaling factor and adjusting your measurement noise covariance matrix (R) instead.

Secondly, your resampling method may not be optimal. Instead of selecting particles based on a random number, I recommend using the systematic resampling method. This method ensures that the particles with higher weights are selected more frequently, thus preventing sample impoverishment.

Lastly, I noticed that you are using roughening in your resampling process. While this may help prevent sample impoverishment, it may also introduce noise to your particles and affect the accuracy of your filter. I suggest removing this step and instead using a larger number of particles to prevent sample impoverishment.

I hope these suggestions will help improve the performance of your particle filter. Please let me know if you have any further questions or if you need any additional help. Good luck with your project!