A Simple Navigation Problem

1. A Simple Navigation Problem By Marc Sobel Work joint with Iyad Obeid and John Montney (Computer and Electrical Engineering)

2. New Model • Discrete time points t=1,….,n • Observe only angles and distances:

3. Applications • Application to: • (1) Terrain navigation; • (2) Tracking Models • (3) Tomographic Models

4. Plot of the Ground Truth and noisy path of the ship

5. Particle Filter. Simplest Method: Assume alpha and beta known. • Generate preliminary t’th generation values from the prior distributions: (k=1,….,100)

6. More Generally • Simulate states from • This usually reduces to:

7. Algorithms • Extended Kalman Filters • Accept Reject algorithms • Auxiliary variables • Mixture methods

8. Weights (simplest story) • Assign the weight (k=1,…,K)

9. Append • Append the t’th values to the already created particles:

10. Resample • Resample the particles • According to the weights;

11. Create Algorithm • function f = create(eta1,eta2,eta3,eta4,alph,bet,x,y,n) • X = cumsum([x alph+eta3*randn(1,n-1)]); • Y = cumsum([y bet+eta4*randn(1,n-1)]); • Z = atan(Y./X) + eta1*randn(size(X)); • d = sqrt(X.^2 + Y.^2) + eta2*randn(size(X)); • plot(X,Y,'r-x'); • hold on • plot(d.*cos(Z),d.*sin(Z),'-+'); • mx = 1.5*max([alph bet])*n; • axis([0 mx 0 mx]); • f = [Z' d' X' Y'];

12. Particle Filter Algorithm • function [fp,fq] = pfreal(a,d,eta1,eta2,eta3,eta4,x,y,alph,bet,n) • nParticles = 500; • p = zeros(nParticles,1); • q = zeros(nParticles,1); • fp = zeros(nParticles,n); • fq = zeros(nParticles,n); • wt_u_prev = ones(nParticles,1); • % initialize all states to x and y, respectively • p(:) = x; fp(:,1) = x; • q(:) = y; fq(:,1) = y; • for ii = 2:n • % inital particle estimate of ii'th generation • p(:) = fp(:,ii-1) + alph + eta3*randn(nParticles,1); • q(:) = fq(:,ii-1) + bet + eta4*randn(nParticles,1); • wt_u_curr = wt_u_prev .* normpdf(a(ii) , atan(q./p) , eta1); • wt_u_curr = wt_u_curr .* normpdf(d(ii) , sqrt(p.^2 + q.^2) , eta2); • wt_n_curr = wt_u_curr ./ sum(wt_u_curr); • in = randsample(1:nParticles,nParticles,true,wt_n_curr); • fp(:,ii) = p(in); • fq(:,ii) = q(in); • wt_u_prev = wt_u_curr; • end

13. Particle Filter Estimates compared with the Ground truth and noisy observations

14. A new idea: use Taylor Series • We get the following expansions:

15. Plug in • Plug these in to get

16. Combine likelihood and prior • Combine the likelihood with prior to get an approximate updated gaussian for the x’s and y’s. Use this updated gaussian as a proposal distribution.