Hi everyone, I am trying to implement in Matlab a particle filter to perform the orbit determination of a spacecraft.(adsbygoogle = window.adsbygoogle || []).push({});

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

---------------------------------------------------------------------------------

**Physics Forums | Science Articles, Homework Help, Discussion**

Dismiss Notice

Join Physics Forums Today!

The friendliest, high quality science and math community on the planet! Everyone who loves science is here!

The friendliest, high quality science and math community on the planet! Everyone who loves science is here!

# Particle filtering for orbit determination

Can you offer guidance or do you also need help?

**Physics Forums | Science Articles, Homework Help, Discussion**