Dismiss Notice
Join Physics Forums Today!
The friendliest, high quality science and math community on the planet! Everyone who loves science is here!

Particle filtering for orbit determination

  1. Sep 13, 2011 #1
    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
    ---------------------------------------------------------------------------------
     
  2. jcsd
Know someone interested in this topic? Share this thread via Reddit, Google+, Twitter, or Facebook

Can you offer guidance or do you also need help?
Draft saved Draft deleted



Similar Discussions: Particle filtering for orbit determination
  1. Filters in CFD (Replies: 2)

  2. Cyclone Filter (Replies: 0)

Loading...