Solving coordinate state estimation using particle filter in python

Question

I have a pickle file which contains 300 coordinates of my subject's location in time. There are some missing values in the middle of it for which I am using a particle filter to estimate those missing values. At the end, I am getting some predictions (not completely accurate) but in a bit drifted form.

So the position of my subject is, in fact, the position of my subject's nose. I take a total of 300 frames and each frame consists of a coordinate for nose in it. There are some frames which have the value of (0,0) meaning the values are missing. So in order to find them, I am implementing the particle filter. I am a newbie for particle filter so there are possibilities that I may have messed up the code. The results that I get, gives me the prediction for 300 frames with drifted values. You can get a clear idea form the image. My measurement value is distance from four landmarks and I provide orientation angle to next point and distance to next point as additional measurements.

from filterpy.monte_carlo import systematic_resample
import numpy as np
import matplotlib.pyplot as plt
from numpy.linalg import norm
from numpy.random import randn
import scipy.stats
from numpy.random import uniform
import pickle
from math import *

#####################################################
def create_uniform_particles(x_range, y_range, hdg_range, N):
    particles = np.empty((N, 3))
    particles[:, 0] = uniform(x_range[0], x_range[1], size=N)
    particles[:, 1] = uniform(y_range[0], y_range[1], size=N)
    particles[:, 2] = uniform(hdg_range[0], hdg_range[1], size=N)
    particles[:, 2] %= 2 * np.pi
    return particles

def create_gaussian_particles(mean, std, N):
    particles = np.empty((N, 3))
    particles[:, 0] = mean[0] + (randn(N) * std[0])
    particles[:, 1] = mean[1] + (randn(N) * std[1])
    particles[:, 2] = mean[2] + (randn(N) * std[2])
    particles[:, 2] %= 2 * np.pi
    return particles


#####################################################
def predict(particles, u, std):
    # move according to control input u (heading change, velocity)
    #with noise Q (std heading change, std velocity)`

    N = len(particles)
    # update heading
    #particles[:, 2] += u[0] + (randn(N) * std[0])
    #particles[:, 2] %= 2 * np.pi
    #u[0] += (randn(N) * std[0])
    u[0] %= 2 * np.pi
    # move in the (noisy) commanded direction
    dist = (u[1]) #+ (randn(N) * std[1])
    particles[:, 0] += np.cos(u[0]) * dist
    particles[:, 1] += np.sin(u[0]) * dist


#####################################################
def update(particles, weights, z, R, landmarks):
    for i, landmark in enumerate(landmarks):
        distance = np.linalg.norm(particles[:, 0:2] - landmark, axis=1)
        weights *= scipy.stats.norm(distance, R).pdf(z[i])

    weights += 1.e-300      # avoid round-off to zero
    weights /= sum(weights) # normalize


#####################################################   
def estimate(particles, weights):
    #returns mean and variance of the weighted particles

    pos = particles[:, 0:2]
    mean = np.average(pos, weights=weights, axis=0)
    var  = np.average((pos - mean)**2, weights=weights, axis=0)
    return mean, var

#####################################################
def simple_resample(particles, weights):
    N = len(particles)
    cumulative_sum = np.cumsum(weights)
    cumulative_sum[-1] = 1. # avoid round-off error
    indexes = np.searchsorted(cumulative_sum, random(N))

    # resample according to indexes
    particles[:] = particles[indexes]
    weights.fill(1.0 / N)
#####################################################
def neff(weights):
    return 1. / np.sum(np.square(weights))

#####################################################
def resample_from_index(particles, weights, indexes):
    particles[:] = particles[indexes]
    weights[:] = weights[indexes]
    weights.fill(1.0 / len(weights))

#####################################################
def read_pickle(pkl_file, f,j):
        with open(pkl_file, 'rb') as res:
            dets = pickle.load(res, encoding = 'latin1')
        all_keyps = dets['all_keyps']
        keyps_t = np.array(all_keyps[1])
        keyps = np.zeros((keyps_t.shape[0], 4, 17))
        for k in range(keyps.shape[0]):
            if keyps_t[k]!=[]:
                keyps[k] = keyps_t[k][0]
        keyps = keyps[:,:2,:]

        for i in range(keyps.shape[0]):
            keyps[i][0] = keyps[i][0]/480*256
            keyps[i][1] = keyps[i][1]/640*256

        x0=keyps[f][0][j]
        y0=keyps[f][1][j]
        x1=keyps[f+1][0][j]
        y1=keyps[f+1][1][j]

        cord = np.array([x0,y0])
        orientation = atan2((y1 - y0),(x1 - x0))
        dist= sqrt((x1-x0) ** 2 + (y1-y0) ** 2)
        u = np.array([orientation,dist])
        return (cord, u)

#####################################################
def run_pf1(N, iters=298, sensor_std_err=.1, 
            do_plot=True, plot_particles=False,
            xlim=(-256, 256), ylim=(-256, 256),
            initial_x=None):
    landmarks = np.array([[0, 0], [0, 256], [256,0], [256,256]])
    NL = len(landmarks)

    plt.figure()

    # create particles and weights
    if initial_x is not None:
        particles = create_gaussian_particles(
            mean=initial_x, std=(5, 5, np.pi/4), N=N)
    else:
        particles = create_uniform_particles((0,20), (0,20), (0, 6.28), N)
    weights = np.ones(N) / N

    if plot_particles:
        alpha = .20
        if N > 5000:
            alpha *= np.sqrt(5000)/np.sqrt(N)           
        plt.scatter(particles[:, 0], particles[:, 1], 
                    alpha=alpha, color='g')

    xs = []
    #robot_pos, u = read_pickle('.pkl',1,0)
    for x in range(iters):
        robot_pos, uv = read_pickle('.pkl',x,0)
        print("orignal: ", robot_pos,)
        # distance from robot to each landmark
        zs = (norm(landmarks - robot_pos, axis=1) + 
              (randn(NL) * sensor_std_err))

        # move diagonally forward to (x+1, x+1)
        predict(particles, u=uv, std=(0, .0))

        # incorporate measurements
        update(particles, weights, z=zs, R=sensor_std_err, 
               landmarks=landmarks)

        # resample if too few effective particles
        if neff(weights) < N/2:
            indexes = systematic_resample(weights)
            resample_from_index(particles, weights, indexes)
            assert np.allclose(weights, 1/N)
        mu, var = estimate(particles, weights)
        #mu +=(120,10)
        xs.append(mu)
        print("expected: ",mu)
        if plot_particles:
            plt.scatter(particles[:, 0], particles[:, 1], 
                        color='k', marker=',', s=1)
        p1 = plt.scatter(robot_pos[0], robot_pos[1], marker='+',
                         color='k', s=180, lw=3)
        p2 = plt.scatter(mu[0], mu[1], marker='s', color='r')
        print(p2)
    xs = np.array(xs)
    #plt.plot(xs[:, 0], xs[:, 1])
    plt.legend([p1, p2], ['Actual', 'PF'], loc=4, numpoints=1)
    plt.xlim(*xlim)
    plt.ylim(*ylim)
    print('final position error, variance:\n\t', mu - np.array([iters, iters]), var)
    plt.show()
    return(p2)

###############################
run_pf1(N=5000)

I expect a set of 300 coordinate values (estimated) as a result of the particle filter so I can replace my missing values in original files with this predicted ones.