Дискретный фильтр.¶

Дискретный фильтр¶

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

Основные компоненты дискретного фильтра¶

Модель системы
Динамическая модель: Описывает, как состояние системы изменяется со временем. \(x_{k+1} = F_k x_k + B_k u_k + w_k\) где \(x_k\) — состояние системы в момент времени \(k\), \(F_k\) — матрица перехода, \(B_k\) — матрица управления, \(u_k\) — вектор управления, \(w_k\) — случайный шум процесса с дисперсией \(Q_k\).
Модель измерений: Описывает, как производятся измерения состояния системы. \(z_k = H_k x_k + v_k\) где \(z_k\) — вектор измерений, \(H_k\) — матрица наблюдения, \(v_k\) — шум измерения с дисперсией \(R_k\).
Процедура предсказания
Предсказание состояния: Оценивается состояние системы на основе предыдущей оценки. \(\hat{x}_{k|k-1} = F_{k-1} \hat{x}_{k-1|k-1} + B_{k-1} u_{k-1}\)
Предсказание ошибки ковариации: Оценивается ковариация ошибки предсказания. \(P_{k|k-1} = F_{k-1} P_{k-1|k-1} F_{k-1}^T + Q_{k-1}\)
Процедура обновления
Вычисление калмановского коэффициента усиления: Определяется вес, с которым новое измерение используется для обновления оценки состояния. \(K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}\)
Обновление оценки состояния: Корректируется предсказанная оценка состояния на основе новых измерений. \(\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1})\)
Обновление ошибки ковариации: Корректируется ковариация ошибки оценки. \(P_{k|k} = (I - K_k H_k) P_{k|k-1}\)

Пример применения дискретного фильтра¶

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

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

Модель системы: - Состояние объекта включает положение и скорость: \(x_k = \begin{pmatrix} p_k \\ v_k \end{pmatrix}\). - Матрица перехода для равномерного движения: \(F_k = \begin{pmatrix} 1 & \Delta t \\ 0 & 1 \end{pmatrix}\), где \(\Delta t\) — шаг дискретизации. - Матрица управления: \(B_k = 0\) (нет управления). - Шум процесса: \(w_k \sim \mathcal{N}(0, Q_k)\), где \(Q_k = \begin{pmatrix} q_{11} & q_{12} \\ q_{21} & q_{22} \end{pmatrix}\).

Модель измерений: - Измеряется только положение объекта: \(z_k = p_k + v_k\). - Матрица наблюдения: \(H_k = \begin{pmatrix} 1 & 0 \end{pmatrix}\). - Шум измерения: \(v_k \sim \mathcal{N}(0, R_k)\), где \(R_k = r\).

Реализация фильтра Калмана¶

Инициализация:
Начальное состояние: \(\hat{x}_{0|0} = \begin{pmatrix} p_0 \\ v_0 \end{pmatrix}\).
Начальная ковариация ошибки: \(P_{0|0} = P_0\).
Предсказание: \(\hat{x}_{k|k-1} = F_{k-1} \hat{x}_{k-1|k-1}\) \(P_{k|k-1} = F_{k-1} P_{k-1|k-1} F_{k-1}^T + Q_{k-1}\)
Обновление: \(K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}\) \(\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1})\) \(P_{k|k} = (I - K_k H_k) P_{k|k-1}\)

Python¶

Вот пример реализации дискретного фильтра Калмана на Python с использованием объектно-ориентированного подхода, чистого кода и комментариев для наглядности:

import numpy as np

class KalmanFilter:
    def __init__(self, initial_state, initial_covariance, process_noise_covariance, measurement_noise_covariance):
        self.state = initial_state
        self.covariance = initial_covariance
        self.process_noise_covariance = process_noise_covariance
        self.measurement_noise_covariance = measurement_noise_covariance

    def predict(self, F, Q):
        # Prediction step
        # Predict state and covariance
        self.state = F @ self.state
        self.covariance = F @ self.covariance @ F.T + Q

    def update(self, z, H, R):
        # Update step
        # Calculate Kalman gain
        K = self.covariance @ H.T @ np.linalg.inv(H @ self.covariance @ H.T + R)
        # Update state estimate
        self.state = self.state + K @ (z - H @ self.state)
        # Update covariance estimate
        self.covariance = (np.eye(len(self.state)) - K @ H) @ self.covariance

# Example usage
# Define initial state and covariance
initial_state = np.array([[0], [0]])  # Initial position and velocity
initial_covariance = np.diag([1, 1])  # Initial uncertainty
# Define process noise covariance and measurement noise covariance
process_noise_covariance = np.diag([0.1, 0.1])  # Process noise
measurement_noise_covariance = np.array([[0.5]])  # Measurement noise

# Create Kalman filter object
kf = KalmanFilter(initial_state, initial_covariance, process_noise_covariance, measurement_noise_covariance)

# Define dynamics matrices
F = np.array([[1, 1], [0, 1]])  # State transition matrix
H = np.array([[1, 0]])          # Observation matrix
# Simulate measurements
measurements = [1, 2, 3, 4, 5]
for z in measurements:
    # Predict
    kf.predict(F, process_noise_covariance)
    # Update with measurement
    kf.update(np.array([[z]]), H, measurement_noise_covariance)
    # Print estimated state
    print("Estimated state:", kf.state.flatten())

Этот пример демонстрирует создание класса KalmanFilter, который содержит методы для предсказания и обновления состояния фильтра. Мы также создаем объект фильтра, определяем модели системы (матрицы F и H) и имитируем измерения.

Заключение¶

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