# Particle filter on more dimensions?

I decided to write and implement a very small radar tracking program in order to understand the basics of the particle filter. So I wrote a class (class Aircraft) for a ideal moving plane which moves with constant speed and constant altitude . The model can be mathematically written as:

$$\underline{x}_{k + 1} = \underline{x}_{k} + \underline{v} \Delta t$$

where $$\underline{x} = [x, y]^T$$ Since speed and altitude are constant:

$$x_{k+1} = x_{k} + v_{0} \Delta t \\ y_{k+1} = h_{0}$$

Then I wrote anothe class (class Radar) to simulate a radar for tracking the aircraft. Basically it takes the x and y coordinates of the aircraft and calculate the range and the slope of the radar beam tracking the airplane. Since a radar is not perfect I added some gaussian noise $$\mathcal{N}(0, \sigma)$$ to the range and to the slope, thus:

$$\rho = \sqrt{x_{k}^{2} + y_{k}^{2}} + \mathcal{N}(0, \sigma)\\ \theta = arctan2(y_{k}, x_{k}) + \mathcal{N}(0, \sigma)$$

Here a screenshot of the program and below the whole code for all the people, who can profit from it: Now...my problem is simply that in my program, at the prediction step, I predict the particles using a model, where speed and altitude are constant all the time -> starting conditions: $$(v_{0}, h_{0})$$

Question: since the model above is not close to the reality (a real aircraft can change speed and altitude at any given time), how can I realize a particle filter, which take account of the position (done), speed and altitude at the same time? Should I implement a particle filter for the speed and for the altitude and then feed the outputs to the particle filter for the position? Or there are other better and more efficient ways to track more then a state with a particle filter?

Here the entire code for playing (many thanks to Robert Labbe and his staff: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python)

#!/usr/bin/env python3

__version__ = "0.1"

# Importing relevant libraries
from filterpy.monte_carlo import systematic_resample, residual_resample, stratified_resample, multinomial_resample

import matplotlib.animation as animation
import matplotlib.pyplot    as plt
import matplotlib

import pandas as pd
import numpy  as np
import scipy.stats

###################################################
#
#  Defining a class for the aircraft
#
###################################################
class Aircraft(object):

def __init__(self, x0, v0, h0):

self.x0 = x0  # horizontal position
self.v0 = v0  # horizontal velocity
self.h0 = h0  # height

def getRealPosition(self, t):
""" Calculate the real position of the aircraft
Remember: it flies at constant speed and height
"""
x = self.x0 + (self.v0 * t)

return np.array([x, self.h0])

###################################################
#
#  Defining a class for the radar
#
###################################################

def __init__(self, sigma):

self.range = 0.0    # direct way to the target
self.theta = 0.0    # angle to the target
self.sigma = sigma  # radar noise

def getRange(self, position):
""" Calculate the range and the angle to the target, and add some noise
"""
range      = np.sqrt(np.square(position) + np.square(position))
self.range = range + (np.random.randn() * self.sigma)

theta      = np.arctan2(position, position)
self.theta = theta + (np.random.randn() * self.sigma / 30.0)

return np.array([self.range, self.theta])

""" Utility to display the target seen by the radar on the plot """
x = data * np.cos(data)
y = data * np.sin(data)

return np.array([x, y])

###################################################
#
#  Defining a class for the particle filter
#
###################################################
class particleFilter(object):

def __init__(self, N, x0, y0):

self.__num_particles = N
self.__particles     = np.empty((N, 2))
self.__x0            = x0
self.__y0            = y0
self.__weights       = np.ones(N) / N

def create_gaussian_particles(self, mean, std):

self.__particles[:, 0] = self.__x0 + (np.random.randn(self.__num_particles) * std)
self.__particles[:, 1] = self.__y0 + (np.random.randn(self.__num_particles) * std)

def predict(self, v0, std):

self.__particles[:, 0] += (v0 * 1.0) + (np.random.randn(self.__num_particles) * std)
self.__particles[:, 1] += (np.random.randn(self.__num_particles) * std)

distance = np.linalg.norm((self.__particles - pos), axis = 1)
self.__weights *= scipy.stats.norm(distance, R).pdf(z)

# Normalize weights
self.__weights += 1.e-300
self.__weights /= sum(self.__weights)

def neff(self):
""" Compute the effective N, which is the number of particle, which contribute to the probability of distribution """
return (1. / np.sum(np.square(self.__weights)))

def resample_from_index(self, indexes):

self.__particles[:] = self.__particles[indexes]

self.__weights.resize(len(self.__particles))
self.__weights.fill(1.0 / len(self.__weights))

def resample(self):

if self.neff() < (self.__num_particles / 2):

indexes = systematic_resample(self.__weights)
self.resample_from_index(indexes)

assert np.allclose(self.__weights, (1. / self.__num_particles))

def estimate(self):
""" Mean and variance of the weighted particles """
pos = self.__particles[::1]

mean = np.average(pos, weights = self.__weights, axis = 0)
var  = np.average((pos - mean) ** 2, weights = self.__weights, axis = 0)

return mean, var

def getParticles(self):

return self.__particles

###################################################
#
#  Here begins the main code
#
###################################################

N_MAX       = 200
N_PARTICLES = 1000
x_map_dim   = N_MAX
y_map_dim   = 80

radar_noise    = 0.8   # noise of the measure

x0 =  0.0  # Starting position
v0 =  1.0  # Aircraft speed
h0 = 56.0  # Aircraft height

aircraft  = Aircraft(x0, v0, h0)
pf_filter = particleFilter(N_PARTICLES, x0, h0)

fig, ax = plt.subplots()

plt.xlim(0.0, x_map_dim)
plt.ylim(0.0, y_map_dim)

pt1, = ax.plot([], [], 'g>', ms = 12)   # Draw the aircraft on the plot
pt2, = ax.plot([], [], 'r.', ms = 4)    # Draw the target seen by the radar on the plot
pt3, = ax.plot([], [], 'c.', ms = 0.2)  # Draw the particles on the plot
pt4, = ax.plot([], [], 'b+', ms = 4)    # Draw the estimated position of the aircraft on the plot

ax.legend([pt1, pt2, pt3, pt4], ['Aircraft', 'Radar', 'Particles', 'Estimated position'], loc = 'lower right')

def animate(i):

real_pos      = aircraft.getRealPosition(i)

pf_filter.predict(v0, radar_noise)  # Prediction made on the basis that speed and height are constant

pf = pf_filter.getParticles()  # Not needed. It is only for showing the particles on the plot

pf_filter.resample()

mu, _ = pf_filter.estimate()

pt1.set_data(real_pos, real_pos)
pt3.set_data(pf[:, 0], pf[:, 1])
pt4.set_data(mu, mu)

animation = matplotlib.animation.FuncAnimation(fig = fig, func = animate, frames = N_MAX, interval = 100, repeat = False)

plt.show()