Фильтр Калмана. Расширенный фильтр Калмана.¶

Pasted image 20240526151833.png

Фильтр Калмана¶

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

Основные шаги фильтра Калмана¶

Предсказание (Prediction):
Предсказание состояния: \(\hat{x}_{k|k-1} = A_{k-1} \hat{x}_{k-1|k-1} + B_{k-1} u_{k-1}\) где \(\hat{x}_{k|k-1}\) — предсказанное состояние в момент времени \(k\), \(A_{k-1}\) — матрица перехода состояния, \(\hat{x}_{k-1|k-1}\) — оценка состояния в момент времени \(k-1\), \(B_{k-1}\) — матрица управления, \(u_{k-1}\) — вектор управления.
Предсказание ковариации ошибки: \(P_{k|k-1} = A_{k-1} P_{k-1|k-1} A_{k-1}^T + Q_{k-1}\) где \(P_{k|k-1}\) — предсказанная ковариация ошибки, \(P_{k-1|k-1}\) — ковариация ошибки в момент времени \(k-1\), \(Q_{k-1}\) — ковариация шума процесса.
Обновление (Update):
Вычисление ошибки измерения: \(y_k = z_k - H_k \hat{x}_{k|k-1}\) где \(y_k\) — инновация или ошибка измерения, \(z_k\) — измерение, \(H_k\) — матрица наблюдения.
Вычисление ковариации ошибки измерения: \(S_k = H_k P_{k|k-1} H_k^T + R_k\) где \(S_k\) — ковариация инновации, \(R_k\) — ковариация шума измерения.
Вычисление коэффициента Калмана: \(K_k = P_{k|k-1} H_k^T S_k^{-1}\) где \(K_k\) — коэффициент Калмана.
Обновление оценки состояния: \(\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k y_k\)
Обновление ковариации ошибки: \(P_{k|k} = (I - K_k H_k) P_{k|k-1}\)

Расширенный фильтр Калмана (EKF)¶

Расширенный фильтр Калмана (EKF) используется для нелинейных систем. Он аппроксимирует нелинейные функции, используя разложение в ряд Тейлора.

Основные шаги расширенного фильтра Калмана¶

Предсказание (Prediction):
Предсказание состояния: \(\hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_{k-1})\) где \(f(\cdot)\) — нелинейная функция перехода состояния.
Предсказание ковариации ошибки: \(P_{k|k-1} = F_{k-1} P_{k-1|k-1} F_{k-1}^T + Q_{k-1}\) где \(F_{k-1} = \frac{\partial f}{\partial x}\big|_{\hat{x}_{k-1|k-1}, u_{k-1}}\) — якобиан функции перехода состояния.
Обновление (Update):
Вычисление ошибки измерения: \(y_k = z_k - h(\hat{x}_{k|k-1})\) где \(h(\cdot)\) — нелинейная функция наблюдения.
Вычисление ковариации ошибки измерения: \(S_k = H_k P_{k|k-1} H_k^T + R_k\) где \(H_k = \frac{\partial h}{\partial x}\big|_{\hat{x}_{k|k-1}}\) — якобиан функции наблюдения.
Вычисление коэффициента Калмана: \(K_k = P_{k|k-1} H_k^T S_k^{-1}\)
Обновление оценки состояния: \(\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k y_k\)
Обновление ковариации ошибки: \(P_{k|k} = (I - K_k H_k) P_{k|k-1}\)

Пример¶

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

Определения и данные¶

Положение и скорость объекта: \(x = \begin{pmatrix} \text{положение} \\ \text{скорость} \end{pmatrix}\)
Измерения: \(z = \text{положение}\)
Переходное состояние: \(A = \begin{pmatrix} 1 & \Delta t \\ 0 & 1 \end{pmatrix}\) где \(\Delta t\) — шаг времени.
Матрица наблюдения: \(H = \begin{pmatrix} 1 & 0 \end{pmatrix}\)
Матрица управления и вектор управления отсутствуют (\(B = 0\) и \(u = 0\)).
ковариации шумов процесса и измерения: \(Q\) и \(R\).

Шаги фильтра Калмана¶

Предсказание: \(\hat{x}_{k|k-1} = A \hat{x}_{k-1|k-1}\)

\(P_{k|k-1} = A P_{k-1|k-1} A^T + Q\)

Обновление: \(y_k = z_k - H \hat{x}_{k|k-1}\)

\(S_k = H P_{k|k-1} H^T + R\)

\(K_k = P_{k|k-1} H^T S_k^{-1}\)

\(\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k y_k\)

\(P_{k|k} = (I - K_k H) P_{k|k-1}\)

Python¶

Фильтр Калмана

class KalmanFilter:
    def __init__(self, initial_state_mean, initial_state_covariance, process_noise_covariance, measurement_noise_covariance):
        self.state_mean = initial_state_mean  # Среднее состояние
        self.state_covariance = initial_state_covariance  # Ковариация состояния
        self.process_noise_covariance = process_noise_covariance  # Ковариация шума процесса
        self.measurement_noise_covariance = measurement_noise_covariance  # Ковариация шума измерения

    def predict(self, control_input=None):
        # Прогнозирование следующего состояния
        # Если у нас есть управляющий вход, учитываем его
        if control_input is not None:
            self.state_mean += control_input

        # Обновляем ковариацию состояния с учетом ковариации шума процесса
        self.state_covariance += self.process_noise_covariance

    def update(self, measurement):
        # Обновление состояния на основе измерения
        # Вычисляем вес Калмана
        kalman_gain = self.state_covariance / (self.state_covariance + self.measurement_noise_covariance)

        # Обновляем состояние
        self.state_mean += kalman_gain * (measurement - self.state_mean)

        # Обновляем ковариацию состояния
        self.state_covariance *= (1 - kalman_gain)

if __name__ == "__main__":
    # Пример использования
    initial_state_mean = 0
    initial_state_covariance = 1
    process_noise_covariance = 0.1
    measurement_noise_covariance = 0.1

    # Создаем фильтр Калмана
    kf = KalmanFilter(initial_state_mean, initial_state_covariance, process_noise_covariance, measurement_noise_covariance)

    # Предсказываем следующее состояние
    kf.predict(control_input=0.2)

    # Получаем измерение
    measurement = 0.6

    # Обновляем состояние фильтра
    kf.update(measurement)

    # Выводим результаты
    print("Updated State Mean:", kf.state_mean)
    print("Updated State Covariance:", kf.state_covariance)

Расширенный фильтр Калмана

import numpy as np

class ExtendedKalmanFilter:
    def __init__(self, initial_state_mean, initial_state_covariance, process_noise_covariance, measurement_noise_covariance, transition_function, observation_function, control_input_function):
        self.state_mean = initial_state_mean  # Среднее состояние
        self.state_covariance = initial_state_covariance  # Ковариация состояния
        self.process_noise_covariance = process_noise_covariance  # Ковариация шума процесса
        self.measurement_noise_covariance = measurement_noise_covariance  # Ковариация шума измерения
        self.transition_function = transition_function  # Функция перехода состояния
        self.observation_function = observation_function  # Функция измерения
        self.control_input_function = control_input_function  # Функция управляющего входа

    def predict(self, control_input=None):
        # Прогнозирование следующего состояния
        # Если у нас есть управляющий вход, учитываем его
        if control_input is not None:
            self.state_mean = self.transition_function(self.state_mean, control_input)
        else:
            self.state_mean = self.transition_function(self.state_mean)

        # Якобиан матрицы перехода состояния
        jacobian_transition = self.compute_jacobian(self.transition_function, self.state_mean)

        # Обновляем ковариацию состояния
        self.state_covariance = np.dot(jacobian_transition, np.dot(self.state_covariance, jacobian_transition.T)) + self.process_noise_covariance

    def update(self, measurement):
        # Обновление состояния на основе измерения
        # Якобиан матрицы измерения
        jacobian_observation = self.compute_jacobian(self.observation_function, self.state_mean)

        # Вычисляем вес Калмана
        kalman_gain = np.dot(self.state_covariance, np.dot(jacobian_observation.T, np.linalg.inv(np.dot(jacobian_observation, np.dot(self.state_covariance, jacobian_observation.T)) + self.measurement_noise_covariance)))

        # Обновляем состояние
        self.state_mean += np.dot(kalman_gain, (measurement - self.observation_function(self.state_mean)))

        # Обновляем ковариацию состояния
        self.state_covariance -= np.dot(kalman_gain, np.dot(jacobian_observation, self.state_covariance))

    def compute_jacobian(self, function, x):
        """Вычисляет Якобиан функции в точке x."""
        epsilon = 1e-6
        n = len(x)
        m = len(function(x))
        jacobian = np.zeros((m, n))
        for i in range(n):
            x_plus_epsilon = np.copy(x)
            x_plus_epsilon[i] += epsilon
            jacobian[:, i] = (function(x_plus_epsilon) - function(x)) / epsilon
        return jacobian

# Пример использования
def transition_function(state, control_input=None):
    # Пример функции перехода состояния (локализация в 1D пространстве)
    if control_input is not None:
        return state + control_input
    else:
        return state

def observation_function(state):
    # Пример функции измерения (наблюдение в 1D пространстве)
    return state**2

def control_input_function():
    # Пример функции управляющего входа
    return np.array([0.2])

if __name__ == "__main__":
    initial_state_mean = np.array([0])  # Начальное среднее состояние
    initial_state_covariance = np.eye(1)  # Начальная ковариация состояния
    process_noise_covariance = np.eye(1) * 0.1  # Ковариация шума процесса
    measurement_noise_covariance = np.eye(1) * 0.1  # Ковариация шума измерения

    # Создаем фильтр Калмана
    ekf = ExtendedKalmanFilter(initial_state_mean, initial_state_covariance, process_noise_covariance, measurement_noise_covariance, transition_function, observation_function, control_input_function)

    # Предсказываем следующее состояние
    ekf.predict()

    # Получаем измерение
    measurement = np.array([1])

    # Обновляем состояние фильтра
    ekf.update(measurement)

    # Выводим результаты
    print("Updated State Mean:", ekf.state_mean)
    print("Updated State Covariance:", ekf.state_covariance)

Заключение¶

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