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

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?

Draft saved
Draft deleted

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