Фильтр частиц.¶

Фильтр частиц¶

фильтр частиц — это вероятностный метод, используемый для оценки состояния динамических систем, в которых нелинейности и/или ненормальности мешают применению традиционных методов, таких как фильтр Калмана. Он основан на использовании множества "частиц" для аппроксимации апостериорного распределения состояния системы.

Основные концепции¶

Частицы
Частицы представляют собой возможные состояния системы и ассоциированные с ними веса.
Каждая частица — это гипотеза о текущем состоянии системы.
Алгоритм фильтра частиц
Инициализация: Создание и инициализация множества частиц.
Предсказание: Обновление состояний частиц на основе динамической модели системы.
Обновление весов: Изменение весов частиц на основе наблюдений.
Ресемплирование: Замена частиц с малыми весами частицами с большими весами, чтобы избежать вырождения частиц.

Шаги алгоритма фильтра частиц¶

Инициализация

Инициализация \(N\) частиц \(\{x_0^{(i)}, w_0^{(i)}\}_{i=1}^N\), где \(x_0^{(i)}\) — начальное состояние \(i\)-й частицы, а \(w_0^{(i)} = \dfrac{1}{N}\) — её вес.

Предсказание

Для каждого момента времени \(k\) обновляется состояние частиц согласно модели динамики:

\(x_k^{(i)} = f(x_{k-1}^{(i)}, u_{k-1}) + w_{k-1}^{(i)}\)

где \(f\) — функция перехода, \(u_{k-1}\) — управляющее воздействие, \(w_{k-1}^{(i)}\) — шум процесса.

Обновление весов

Вес частицы обновляется на основе наблюдений: \(w_k^{(i)} \propto w_{k-1}^{(i)} \cdot p(z_k | x_k^{(i)})\) где \(p(z_k | x_k^{(i)})\) — вероятность наблюдения \(z_k\), учитывая состояние частицы \(x_k^{(i)}\).

Нормализация весов

Нормализуем веса частиц, чтобы их сумма равнялась 1: \(w_k^{(i)} = \dfrac{w_k^{(i)}}{\sum_{j=1}^N w_k^{(j)}}\)

Ресемплирование

Если веса частиц сильно разнятся, то проводится ресемплирование. Частицы с малыми весами заменяются на частицы с большими весами, чтобы сохранить численную стабильность: \(\{x_k^{(i)}\}_{i=1}^N \sim \{x_k^{(i)}, w_k^{(i)}\}_{i=1}^N\) После ресемплирования веса частиц устанавливаются равными: \(w_k^{(i)} = \dfrac{1}{N}\).

Пример применения фильтра частиц¶

Рассмотрим задачу отслеживания положения и скорости объекта, движущегося по прямой, с использованием фильтра частиц.

Описание задачи: - Объект движется с постоянной скоростью. - Измерения положения объекта подвержены случайным ошибкам.

Модель системы: - Состояние объекта включает положение и скорость: \(x_k = \begin{pmatrix} p_k \\ v_k \end{pmatrix}\). - Модель динамики: \(f(x_{k-1}, u_{k-1}) = \begin{pmatrix} p_{k-1} + \Delta t \cdot v_{k-1} \\ v_{k-1} \end{pmatrix}\). - Шум процесса: \(w_k \sim \mathcal{N}(0, Q)\), где \(Q\) — ковариация шума процесса.

Модель измерений: - Измеряется только положение объекта: \(z_k = p_k + v_k\). - Шум измерения: \(v_k \sim \mathcal{Н}(0, R)\), где \(R\) — дисперсия шума измерения.

Реализация фильтра частиц¶

Инициализация:
Создание \(N\) частиц \(\{x_0^{(i)}, w_0^{(i)}\}_{i=1}^N\), где \(x_0^{(i)} \sim p(x_0)\) и \(w_0^{(i)} = \dfrac{1}{N}\).
Предсказание: \(x_k^{(i)} = F x_{k-1}^{(i)} + w_{k-1}^{(i)}\) где \(F = \begin{pmatrix} 1 & \Delta t \\ 0 & 1 \end{pmatrix}\).
Обновление весов: \(w_k^{(i)} \propto w_{k-1}^{(i)} \cdot \exp\left(-\dfrac{(z_k - H x_k^{(i)})^2}{2R}\right)\) где \(H = \begin{pmatrix} 1 & 0 \end{pmatrix}\).
Нормализация весов: \(w_k^{(i)} = \dfrac{w_k^{(i)}}{\sum_{j=1}^N w_k^{(j)}}\)
Ресемплирование:
Если эффективное число частиц \(N_{\text{eff}} = \dfrac{1}{\sum_{i=1}^N (w_k^{(i)})^2}\) меньше заданного порога, выполняется ресемплирование.
Новые частицы выбираются с вероятностью, пропорциональной их весам.

Python¶

import numpy as np

class ParticleFilter:
    def __init__(self, num_particles, initial_state, process_noise):
        """
        Initializes the particle filter.

        Args:
            num_particles (int): Number of particles.
            initial_state (numpy.ndarray): Initial state estimate.
            process_noise (numpy.ndarray): Process noise covariance matrix.
        """
        self.num_particles = num_particles
        self.state = initial_state
        self.process_noise = process_noise
        self.particles = np.random.multivariate_normal(initial_state, process_noise, num_particles)

    def predict(self, motion_model):
        """
        Predicts the next state of particles.

        Args:
            motion_model (function): Motion model function.

        Returns:
            numpy.ndarray: Predicted particles.
        """
        self.particles = motion_model(self.particles)
        return self.particles

    def update(self, measurement, measurement_model):
        """
        Updates the particles based on measurement.

        Args:
            measurement (numpy.ndarray): Measurement vector.
            measurement_model (function): Measurement model function.

        Returns:
            numpy.ndarray: Updated particles.
        """
        weights = np.apply_along_axis(measurement_model, 1, self.particles, measurement)
        weights += 1.e-300  # Add small value to avoid division by zero
        weights /= np.sum(weights)
        indices = np.random.choice(np.arange(self.num_particles), size=self.num_particles, replace=True, p=weights)
        self.particles = self.particles[indices]
        return self.particles

# Example motion model
def motion_model(particles):
    """
    Example motion model.

    Args:
        particles (numpy.ndarray): Particles.

    Returns:
        numpy.ndarray: Predicted particles.
    """
    # Add some random noise to particles
    noise = np.random.multivariate_normal(np.zeros(particles.shape[1]), np.eye(particles.shape[1]), particles.shape[0])
    particles += noise
    return particles

# Example measurement model
def measurement_model(particle, measurement):
    """
    Example measurement model.

    Args:
        particle (numpy.ndarray): Particle.
        measurement (numpy.ndarray): Measurement vector.

    Returns:
        float: Weight of the particle.
    """
    # Example: calculate weight based on Euclidean distance
    return np.exp(-np.linalg.norm(particle - measurement)**2)

# Example usage
num_particles = 1000
initial_state = np.array([0, 0])  # Initial state estimate
process_noise = np.eye(2) * 0.01  # Process noise covariance matrix
pf = ParticleFilter(num_particles, initial_state, process_noise)

# Simulate motion and measurement
true_state = np.array([1, 1])  # True state
measurement = true_state + np.random.normal(0, 0.1, 2)  # Simulated measurement

# Predict and update
predicted_particles = pf.predict(motion_model)
updated_particles = pf.update(measurement, measurement_model)

Заключение¶

Фильтр частиц предоставляет мощный инструмент для оценки состояния динамических систем, особенно в условиях нелинейности и ненормальности. Он широко применяется в задачах робототехники, навигации, компьютерного зрения и других областях, где традиционные методы оценки состояния могут быть неэффективны.