Кибернетика, автоматика, протоколы

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilterPressureWithAccel:
    def __init__(self, initial_pressure=0.0, initial_velocity=0.0, initial_acceleration=0.0, dt=1.0, 
                 process_noise_std=0.1, measurement_noise_std=1.0):
        """
        Фильтр Калмана для показаний датчика давления с учетом ускорения.
        
        Эта модель описывает давление как состояние с позицией (давление), скоростью (скорость изменения давления)
        и ускорением (скорость изменения скорости). Учитывает внешние воздействия, позволяя шуму процесса
        влиять на ускорение (модель с белым шумом рывка).
        
        Параметры:
        - initial_pressure: Начальная оценка давления.
        - initial_velocity: Начальная оценка скорости изменения давления.
        - initial_acceleration: Начальная оценка ускорения (вторая производная).
        - dt: Шаг времени между измерениями.
        - process_noise_std: Стандартное отклонение шума процесса (влияет на рывок, изменение ускорения).
        - measurement_noise_std: Стандартное отклонение шума измерений.
        """
        # Вектор состояния: [давление, скорость, ускорение]^T
        self.x = np.array([[initial_pressure], [initial_velocity], [initial_acceleration]])
        
        # Начальная ковариационная матрица (неопределенность)
        self.P = np.eye(3) * 10.0  # Начальная неопределенность
        
        # Матрица перехода состояний (модель постоянного ускорения)
        self.F = np.array([[1, dt, 0.5 * dt**2],
                           [0, 1, dt],
                           [0, 0, 1]])
        
        # Матрица измерений (измеряется только давление)
        self.H = np.array([[1, 0, 0]])
        
        # Ковариационная матрица шума процесса (модель белого шума рывка)
        q = process_noise_std ** 2
        self.Q = q * np.array([[dt**5 / 20, dt**4 / 8, dt**3 / 6],
                               [dt**4 / 8,  dt**3 / 3, dt**2 / 2],
                               [dt**3 / 6,  dt**2 / 2, dt]])
        
        # Ковариационная матрица шума измерений
        self.R = np.array([[measurement_noise_std ** 2]])
    
    def predict(self):
        """Предсказать следующее состояние на основе модели."""
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q
    
    def update(self, measurement):
        """Обновить состояние на основе нового измерения."""
        z = np.array([[measurement]])
        y = z - self.H @ self.x  # Инновация
        S = self.H @ self.P @ self.H.T + self.R  # Ковариация инновации
        K = self.P @ self.H.T @ np.linalg.inv(S)  # Коэффициент усиления Калмана
        self.x = self.x + K @ y
        I = np.eye(3)
        self.P = (I - K @ self.H) @ self.P
    
    def get_pressure(self):
        """Получить текущее оцененное давление."""
        return self.x[0, 0]

# Пример использования с визуализацией
if __name__ == "__main__":
    # Симуляция измерений давления с шумом и изменениями
    np.random.seed(42)  # Для воспроизводимости
    time_steps = np.arange(0, 10, 1)
    true_pressures = [100, 102, 105, 107, 110, 108, 105, 103, 100, 98]  # Изменения из-за внешних факторов
    measurements = [p + np.random.normal(0, 1) for p in true_pressures]  # Шумные измерения
    
    # Инициализация фильтра Калмана
    kf = KalmanFilterPressureWithAccel(initial_pressure=100.0, dt=1.0, process_noise_std=0.5, measurement_noise_std=1.0)
    
    # Запуск фильтра
    filtered = []
    for z in measurements:
        kf.predict()
        kf.update(z)
        filtered.append(kf.get_pressure())
    
    # Вывод результатов
    print("Измерения:", measurements)
    print("Отфильтрованные давления:", filtered)
    
    # Визуализация
    plt.figure(figsize=(10, 6))
    plt.plot(time_steps, true_pressures, 'b-', label='Истинное давление', linewidth=2)
    plt.plot(time_steps, measurements, 'ro', label='Измеренное давление', markersize=5)
    plt.plot(time_steps, filtered, 'g--', label='Отфильтрованное давление', linewidth=2)
    plt.xlabel('Время (с)')
    plt.ylabel('Давление')
    plt.title('Фильтр Калмана для показаний датчика давления')
    plt.legend()
    plt.grid(True)
    plt.show()