3 / main

import numpy as np
import matplotlib.pyplot as plt

# --- Параметры для Варианта 3 (Sprott Case I) ---
H_STEP = 0.1           # Шаг интегрирования
SIGMA_W = 0.001        # СКО шума процесса
SIGMA_V = 0.02         # СКО шума наблюдения

# Начальные условия
X0_SIM = np.array([0.1, 0.0, 0.0])       # Для идеального моделирования
X0_FILTER = np.array([0.1, 0.1, 0.1])    # Начальное приближение фильтра

# Ковариационные матрицы
Q = np.eye(3) * (SIGMA_W ** 2)  # Ковариация шума процесса
R = np.eye(3) * (SIGMA_V ** 2)  # Ковариация шума наблюдения
P0 = np.eye(3)                  # Начальная ковариация ошибок

def sprott_case_i(state):
    """Правая часть системы дифференциальных уравнений Sprott Case I"""
    x, y, z = state
    dxdt = -0.2 * y
    dydt = x + z
    dzdt = x + y**2 - z
    return np.array([dxdt, dydt, dzdt])

def get_jacobian(state, h):
    """
    Вычисление матрицы Якоби F_k для дискретного шага (метод Эйлера)
    F_k = I + h * J_continuous
    """
    x, y, z = state
    
    # Якобиан непрерывной системы
    jac_cont = np.array([
        [0.0, -0.2, 0.0],
        [1.0,  0.0, 1.0],
        [1.0,  2*y, -1.0]
    ])
    
    return np.eye(3) + h * jac_cont

def euler_step(state, h):
    """Интегрирование методом Эйлера"""
    derivs = sprott_case_i(state)
    return state + h * derivs

def read_data(filename):
    """Чтение экспериментальных данных из файла"""
    data = []
    with open(filename, 'r') as f:
        for line in f:
            parts = line.strip().split()
            if parts:
                data.append([float(p) for p in parts])
    return np.array(data)

def run_simulation_ideal(steps):
    """Моделирование идеальной траектории (без учета шумов)"""
    trajectory = [X0_SIM]
    current_state = X0_SIM.copy()
    for _ in range(steps - 1):
        current_state = euler_step(current_state, H_STEP)
        trajectory.append(current_state)
    return np.array(trajectory)

def kalman_filter(measurements):
    """Реализация расширенного фильтра Калмана (EKF)"""
    n_steps = len(measurements)
    filtered_states = []
    
    x_hat = X0_FILTER.copy()
    P = P0.copy()
    
    # Матрица наблюдения
    H_mat = np.eye(3) 
    I = np.eye(3)
    
    for k in range(n_steps):
        y_k = measurements[k]
        
        # --- 1. Предиктор (Time Update) ---
        x_hat_minus = euler_step(x_hat, H_STEP)
        
        F_k = get_jacobian(x_hat, H_STEP)
        P_minus = F_k @ P @ F_k.T + Q
        
        # --- 2. Корректор (Measurement Update) ---
        S = H_mat @ P_minus @ H_mat.T + R
        K_k = P_minus @ H_mat.T @ np.linalg.inv(S)
        
        innovation = y_k - (H_mat @ x_hat_minus)
        x_hat = x_hat_minus + K_k @ innovation
        
        P = (I - K_k @ H_mat) @ P_minus
        
        filtered_states.append(x_hat)
        
    return np.array(filtered_states)

def plot_results(sim_data, meas_data, filt_data):
    """Построение и сохранение графиков."""
    steps = len(meas_data)
    time = np.arange(steps) * H_STEP
    
    # График 1: Временная диаграмма
    plt.figure(figsize=(10, 8))
    var_names = ['x', 'y', 'z']
    
    for i in range(3):
        plt.subplot(3, 1, i+1)
        plt.plot(time, sim_data[:, i], label='Simulation (Ideal)', linewidth=1.5)
        plt.plot(time, meas_data[:, i], label='Data (Noisy)', color='black', alpha=0.6, linewidth=0.8)
        plt.plot(time, filt_data[:, i], label='Filtered (EKF)', color='red', linestyle='--', linewidth=1.5)
        plt.ylabel(var_names[i])
        plt.legend(loc='upper right', fontsize='small')
        plt.grid(True)
        if i == 0:
            plt.title('Временная диаграмма: Sprott Case I')
    
    plt.xlabel('Время (t)')
    plt.tight_layout()
    plt.savefig('F1.png')
    
    # График 2: Фазовый портрет
    plt.figure(figsize=(8, 8))
    plt.plot(sim_data[:, 0], sim_data[:, 1], label='Simulation', linewidth=2)
    plt.plot(meas_data[:, 0], meas_data[:, 1], label='Data', color='black', linewidth=1.8)
    plt.plot(filt_data[:, 0], filt_data[:, 1], label='Filtered', color='red', linewidth=1.5)
    
    plt.title('Фазовый портрет (X vs Y)')
    plt.xlabel('x')
    plt.ylabel('y')
    plt.legend()
    plt.grid(True)
    plt.axis('equal')
    plt.savefig('F2.png')
    
    plt.show()

def main():
    filename = 'data3.txt'
    
    try:
        measurements = read_data(filename)
    except FileNotFoundError:
        print(f"Ошибка: Файл {filename} не найден.")
        return

    steps = len(measurements)
    
    simulation_data = run_simulation_ideal(steps)
    
    filtered_data = kalman_filter(measurements)
    
    plot_results(simulation_data, measurements, filtered_data)

if __name__ == "__main__":
    main()