TONGUE_KEVIN_JN_python/src/kalman/__init__.py

import streamlit as st
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
from scipy import linalg
import plotly.graph_objects as go
from plotly.subplots import make_subplots
import time

def show_kalman_filter():
    """Implémentation pédagogique du filtre de Kalman"""
    
    st.header("🔍 Implémentation du Filtre de Kalman")
    
    # Théorie
    with st.expander("📖 Théorie du filtre de Kalman", expanded=True):
        col1, col2 = st.columns(2)
        
        with col1:
            st.markdown("""
            ### Équations de prédiction
            
            ```python
            # Prédiction de l'état
            x_pred = F @ x_est + B @ u
            
            # Prédiction de la covariance
            P_pred = F @ P_est @ F.T + Q
            ```
            """)
        
        with col2:
            st.markdown("""
            ### Équations de correction
            
            ```python
            # Innovation
            y_residual = z - H @ x_pred
            
            # Covariance de l'innovation
            S = H @ P_pred @ H.T + R
            
            # Gain de Kalman
            K = P_pred @ H.T @ np.linalg.inv(S)
            
            # Mise à jour
            x_est = x_pred + K @ y_residual
            P_est = (I - K @ H) @ P_pred
            ```
            """)
    
    # Interface interactive
    st.subheader("🎮 Simulateur interactif")
    
    col1, col2 = st.columns(2)
    
    with col1:
        st.markdown("### Paramètres du système")
        
        # Système masse-ressort 1D
        mass = st.number_input("Masse (kg)", 0.1, 10.0, 1.0, 0.1)
        stiffness = st.number_input("Rigidité (N/m)", 1.0, 100.0, 10.0, 1.0)
        damping = st.number_input("Amortissement (Ns/m)", 0.0, 5.0, 0.1, 0.1)
        
        dt = st.slider("Pas de temps (s)", 0.001, 0.1, 0.01, 0.001)
    
    with col2:
        st.markdown("### Paramètres du filtre")
        
        process_noise = st.slider("Bruit de processus (Q)", 1e-6, 1.0, 0.01, 0.001,
                                 format="%.4f")
        measurement_noise = st.slider("Bruit de mesure (R)", 1e-6, 1.0, 0.1, 0.001,
                                     format="%.4f")
        
        initial_uncertainty = st.slider("Incertitude initiale (P0)", 0.1, 10.0, 1.0, 0.1)
    
    # Bouton de simulation
    if st.button("🚀 Lancer la simulation", type="primary"):
        simulate_kalman_filter(mass, stiffness, damping, dt, 
                              process_noise, measurement_noise, initial_uncertainty)

def simulate_kalman_filter(mass, stiffness, damping, dt, Q, R, P0):
    """Simule un système masse-ressort avec filtre de Kalman"""
    
    # Matrices du système continu
    A_cont = np.array([[0, 1],
                       [-stiffness/mass, -damping/mass]])
    B_cont = np.array([[0], [1/mass]])
    
    # Discrétisation
    A_disc = np.eye(2) + A_cont * dt
    B_disc = B_cont * dt
    
    # Matrices de mesure
    H = np.array([[1, 0]])  # On mesure seulement la position
    
    # Initialisation
    n_steps = 500
    x_true = np.zeros((2, n_steps))
    x_est = np.zeros((2, n_steps))
    measurements = np.zeros(n_steps)
    
    # État initial
    x_true[:, 0] = [0, 0]
    x_est[:, 0] = [0, 0]
    P = np.eye(2) * P0
    
    # Simulation
    progress_bar = st.progress(0)
    
    for k in range(1, n_steps):
        # Progression
        if k % 50 == 0:
            progress_bar.progress(k / n_steps)
        
        # === Système réel (avec bruit) ===
        # Force d'excitation (sinusoïdale + bruit)
        force = 10 * np.sin(0.5 * k * dt) + np.random.normal(0, np.sqrt(Q))
        
        # Évolution du système
        w = np.random.multivariate_normal([0, 0], np.eye(2) * Q)
        x_true[:, k] = A_disc @ x_true[:, k-1] + B_disc.flatten() * force + w
        
        # Mesure (avec bruit)
        v = np.random.normal(0, np.sqrt(R))
        measurements[k] = (H @ x_true[:, k] + v)[0]
        
        # === Filtre de Kalman ===
        # Prédiction
        x_pred = A_disc @ x_est[:, k-1] + B_disc.flatten() * force
        P_pred = A_disc @ P @ A_disc.T + np.eye(2) * Q
        
        # Correction
        y_residual = measurements[k] - H @ x_pred
        S = H @ P_pred @ H.T + R
        K = P_pred @ H.T / S
        
        x_est[:, k] = x_pred + K.flatten() * y_residual
        P = (np.eye(2) - K @ H) @ P_pred
    
    progress_bar.progress(1.0)
    
    # Visualisation
    fig = make_subplots(rows=2, cols=2,
                       subplot_titles=("Position estimée vs réelle", 
                                      "Vitesse estimée vs réelle",
                                      "Erreur d'estimation", 
                                      "Gain de Kalman"),
                       vertical_spacing=0.15)
    
    t = np.arange(n_steps) * dt
    
    # Position
    fig.add_trace(go.Scatter(x=t, y=x_true[0, :], name='Position réelle',
                            line=dict(color='blue', dash='dash')),
                 row=1, col=1)
    fig.add_trace(go.Scatter(x=t, y=x_est[0, :], name='Position estimée',
                            line=dict(color='red')),
                 row=1, col=1)
    fig.add_trace(go.Scatter(x=t, y=measurements, name='Mesures',
                            mode='markers', marker=dict(size=3, color='gray')),
                 row=1, col=1)
    
    # Vitesse
    fig.add_trace(go.Scatter(x=t, y=x_true[1, :], name='Vitesse réelle',
                            line=dict(color='blue', dash='dash')),
                 row=1, col=2)
    fig.add_trace(go.Scatter(x=t, y=x_est[1, :], name='Vitesse estimée',
                            line=dict(color='red')),
                 row=1, col=2)
    
    # Erreur
    position_error = x_true[0, :] - x_est[0, :]
    fig.add_trace(go.Scatter(x=t, y=position_error, name='Erreur position',
                            line=dict(color='purple')),
                 row=2, col=1)
    fig.add_trace(go.Scatter(x=t, y=np.zeros_like(t), 
                            line=dict(color='black', dash='dot')),
                 row=2, col=1)
    
    # Covariance
    fig.add_trace(go.Scatter(x=t, y=np.sqrt(np.abs(P[0, 0]) * np.ones(n_steps)),
                            name='Écart-type estimé',
                            line=dict(color='orange')),
                 row=2, col=2)
    
    fig.update_layout(height=600, showlegend=True)
    st.plotly_chart(fig, use_container_width=True)
    
    # Métriques de performance
    col1, col2, col3 = st.columns(3)
    
    with col1:
        rmse_position = np.sqrt(np.mean(position_error**2))
        st.metric("RMSE Position", f"{rmse_position:.4f} m")
    
    with col2:
        velocity_error = np.sqrt(np.mean((x_true[1, :] - x_est[1, :])**2))
        st.metric("RMSE Vitesse", f"{velocity_error:.4f} m/s")
    
    with col3:
        max_error = np.max(np.abs(position_error))
        st.metric("Erreur maximale", f"{max_error:.4f} m")