Particle filter on more dimensions?

Question

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
#
###################################################
class Radar(object):

    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
            to the calculation to make the radar reading noisy
        """
        range      = np.sqrt(np.square(position[0]) + np.square(position[1]))
        self.range = range + (np.random.randn() * self.sigma)
        
        theta      = np.arctan2(position[1], position[0])
        self.theta = theta + (np.random.randn() * self.sigma / 30.0)

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


    def getPlotRadar(self, data):
        """ Utility to display the target seen by the radar on the plot """
        x = data[0] * np.cos(data[1])
        y = data[0] * np.sin(data[1])

        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)


    def update(self, z, R, radar_position):

        for i, pos in enumerate(radar_position):

            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
radar_position = [0.0, 0.0]

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


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

pf_filter.create_gaussian_particles([x0, h0], radar_noise)

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)
    radar_measure = radar.getRange(real_pos)
    plot_radar    = radar.getPlotRadar(radar_measure)  # Not needed, if the radar should not be plotted

    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.update(radar_measure[0], radar_noise, radar_position)
    pf_filter.resample()

    mu, _ = pf_filter.estimate()

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


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

plt.show()

Answer 1

Each particle should represent a point in the state space that you care about. So your particles will have information about position (altitude included) and speed.

You should then predict the particles forward using the more accurate model you have. If you don't have any information about how the speed or altitude change, that's okay, just make sure that your measurement noise covariance parameter is enough to capture some of the deviation from the prediction. Of course since you are simulating the sensors, you can just reduce the noise, but in reality, you can't do that.

For the prediction step, each particle will use whatever velocity it has to predict its position. In this way, particles can represent different velocities and hopefully their mean represents the true velocity of the aircraft.