clear all;
clc;


% Chargement des données depuis un fichier Excel
opts = detectImportOptions('Log6.xlsx');
opts.VariableNamingRule = 'preserve';
data = readtable('Log6.xlsx', opts);

params.R0 = 0.023 ;
params.R1 = 0.05985;
params.C1 = 7345.99;
params.R2 = 0.0068;
params.C2 = 1073.25;

%params.R0 =  0.0232 ;
%params.R1 = 0.0482;
%params.C1 = 7345.99;
%params.R2 = 0.0032 ;
%params.C2 = 1099.9982;

% Définition des paramètres de la batterie
%params.R0 = 0.023;    
%params.R1 = 0.007296;    
%params.C1 = 3814.045;    
%params.R2 = 0.002747;   
%params.C2 = 1265.502; 

params.C_b = 18000;   %capacité de la batterie
dt = 0.2;  %temps d'échantillonage   
soc_init = 0.872;

% Initialisation de l'état et de la covariance
x = [soc_init; 0; 0];  % SoC initial, V_RC1 et V_RC2 initialisés à 0
P = eye(3)*1e-6;   % Matrice de covariance initiale avec régularisation

% Covariance des bruits
Q = eye(3)*1e-9;  % Bruit de processus ajusté
R = 0.1;  % Bruit de mesure ajusté

% Simulation du système avec données de courant et de tension observées
current = data.Current; %courant de décharge
voltage_measured = data.Voltage;  %tension de décharge
soc_real = data.SOC;             % SoC réel depuis le tableau Logpoly3, soc de la methode de notre client
n_steps = length(current); 

% Initialisation des matrices pour stocker les SoC estimés par UKF et Coulomb Counting
SoC_UKF_estimates = zeros(n_steps, 1);
P_UKF_estimates = zeros(size(P, 1), size(P, 2), n_steps);


for k = 1:n_steps
   
    
    % Filtre UKF : Appel de la fonction de Prédiction et mise à jour, 
    [x, P] = ukf(@(x) f_battery(x, current(k), params, dt), ...
                 @(x) h_battery(x, current(k), params), ...
                 x, P, Q, R, voltage_measured(k)); 
             
      
    % Contrainte pour s'assurer que le SoC reste entre 0 et 1
    %x(1) = max(0, min(1, x(1))); %soc reste entre 0 et 1
    
    % Stockage de l'estimation UKF du SoC
    SoC_UKF_estimates(k) = x(1);
    % Stockage de la matrice de covariance P
    P_UKF_estimates(:, :, k) = P;
    

end

% Conversion en pourcentage des estimations de l'UKF (entre 0 et 100 %)
SoC_UKF_estimates = SoC_UKF_estimates * 100;  % Conversion en pourcentage

% Méthode Coulomb Counting
SoC_Coulomb = Coulomb_Counting(current, params.C_b, soc_init*100);


% Affichage des résultats complets avec SoC Coulomb après calcul
for k = 1:n_steps
    % Print details about the current, SoC UKF estimate, and Coulomb estimate
    fprintf('Étape %d: Courant = %.4f A, SoC estimé UKF = %.4f%%, SoC estimé Coulomb = %.4f%%\n', ...
        k, current(k), SoC_UKF_estimates(k), SoC_Coulomb(k));
    
    
end


% Calcul du RMSE entre l'UKF et Coulomb Counting
RMSE = sqrt(mean((SoC_UKF_estimates - SoC_Coulomb').^2)); % Vérifier les dimensions correctes
fprintf('RMSE entre UKF et Coulomb Counting: %.6f\n', RMSE);

% Calcul du RMSE entre le SoC estimé par UKF et le SoC réel calculé par la
% méthode de notre client
RMSE_UKF_SOC = sqrt(mean((SoC_UKF_estimates - soc_real).^2));
fprintf('RMSE entre SoC estimé par UKF et SoC réel calculé par la méthode de notre client: %.6f\n', RMSE_UKF_SOC);




% Tracé des estimations de SoC des deux méthodes
figure;
hold on;
plot(SoC_UKF_estimates, 'b', 'LineWidth', 1.5);
plot(SoC_Coulomb, 'r--', 'LineWidth', 1.5);
ylim([0 100]); % Limite les valeurs entre 0 et 100 %
xlabel('Time Step');
ylabel('SoC Estimate (%)');
title('SoC Estimation: UKF vs Coulomb Counting');
legend('UKF Estimation', 'Coulomb Counting');
grid on;
hold off;


% Tracé des estimations de SoC par UKF et du SoC réel
figure;
hold on;
plot(SoC_UKF_estimates, 'b', 'LineWidth', 1.5);
plot(soc_real, 'g--', 'LineWidth', 1.5);
ylim([0 100]);  % Limite les valeurs entre 0 et 100 %
xlabel('Time Step');
ylabel('SoC Estimate (%)');
title('SoC Estimation: UKF vs Real SoC');
legend('UKF Estimation', 'Real SoC');
grid on;
hold off;


%%%%%%%%%%%%%%%%%%% Modèles et fonctions UKF %%%%%%%%%%%%%%%%%%%

% Modèle de transition d'état de la batterie avec bruit réduit
function x_next = f_battery(x, I, params, dt)
    SoC = x(1);          % État de charge de la batterie
    V_RC1 = x(2);        % Tension de la première branche du modèle 2RC de la batterie
    V_RC2 = x(3);        % Tension de la deuxième branche du modèle 2RC de la batterie   


    SoC_next = SoC + (I / params.C_b) * dt;  % Mise à jour de l'état de charge (SoC)
    SoC_next = max(0, min(1, SoC_next)); 

    V_RC1_next = exp(-dt / (params.R1 * params.C1)) * V_RC1 + params.R1 * (1 - exp(-dt / (params.R1 * params.C1))) * I; % Mise à jour de la tension V_RC1 en utilisant une équation exponentielle
    V_RC2_next = exp(-dt / (params.R2 * params.C2)) * V_RC2 + params.R2 * (1 - exp(-dt / (params.R2 * params.C2))) * I; % Mise à jour de la tension V_RC2 en utilisant une équation exponentielle

    x_next = [SoC_next; V_RC1_next; V_RC2_next]; %Construction du vecteur d'état mis à jour contenant SoC, V_RC1, et V_RC2
end

% Modèle de mesure de la batterie avec bruit réduit
function V = h_battery(x, I, params)
    SoC = x(1);          % État de charge de la batterie
    V_RC1 = x(2);        % Tension de la première branche du modèle 2RC de la batterie
    V_RC2 = x(3);        % Tension de la deuxième branche du modèle 2RC de la batterie

    OCV = OCV_from_SOC(SoC);  % Calculer la tension en circuit ouvert (OCV) en fonction de l'état de charge (SoC)
    V = OCV - V_RC1 - V_RC2 - I * params.R0; %la tension estimé utilisé pour calculer l'erreur

end


% Fonction pour calculer l'OCV en fonction du SoC
function OCV = OCV_from_SOC(SoC)
    % Courbe de SoC vs V_OC (données fournies)
    soc_values = [0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100] / 100; % SoC en fraction
    ocv_values = [2.7668, 3.2943, 3.4666, 3.5866, 3.6586, 3.7473, 3.854, 3.9416, 4.051, 4.0824, 4.1257]; % Tensions en Volts

    % Interpolation pour calculer l'OCV en fonction du SoC
    OCV = interp1(soc_values, ocv_values, SoC, 'linear', 'extrap');
end

% definition de la Fonction du filtre UKF : prédiction et mise a jour
function [x_hat, P] = ukf(f, h, x, P, Q, R, z)
    n = numel(x);       %la dimension du vecteur x  
    kappa = 0;         
    alpha = 0.002;          
    beta = 2;             
    
    lambda = alpha^2 * (n + kappa) - n; %ce paramétre est utilisé pour déterminer la dispersion des points sigma
    gamma = sqrt(n + lambda);           
    
    X = sigmaPoints(x, P, gamma); %Point sigma
    
    X_pred = zeros(n, 2*n+1); %initialiser une matrice vide appelée X_pred qui va stocker les points sigma prédits
    for i = 1:2*n+1
        X_pred(:,i) = f(X(:,i));  % la prédiction de chaque point sigma à travers le modéle de la batterie, propagation du point sigma à l'aide de la fonction non linéaire f, 
    end
    
    Wm = [lambda / (n + lambda), repmat(1 / (2 * (n + lambda)), 1, 2 * n)]; %Le poid moyen pour calculer la moyenne pondérée des points sigma
    Wc = Wm; %initialement, les poids de covariance sont les mêmes que les poids de moyenne.
    Wc(1) = Wc(1) + (1 - alpha^2 + beta); %poid de covariance pour calculer la covariance pondérée des points sigma
    
    x_pred = X_pred * Wm';  %l'estimation de l'état moyen après la propagation des points sigma à travers la fonction de transition d'état non linéaire f
    P_pred = Q; %initialisation de la covariance avec le bruit de processus
    for i = 1:2*n+1
        P_pred = P_pred + Wc(i) * (X_pred(:,i) - x_pred) * (X_pred(:,i) - x_pred)'; %calcul de la covariance
    end
    
    Z_pred = zeros(numel(z), 2*n+1); %Initialiser une matrice vide Z_pred qui va contenir les valeurs prédictives des mesures pour chaque point sigma
    for i = 1:2*n+1
        Z_pred(:,i) = h(X_pred(:,i));  %Stocke la mesure prédite pour le i-ème point sigma dans la colonne correspondante de Z_pred
    end
    
    z_pred = Z_pred * Wm';  %la mesure moyenne prédite, tension prédite
    
    Pz = R; %initialisation de la covariance par le bruit de mesure
    for i = 1:2*n+1
        Pz = Pz + Wc(i) * (Z_pred(:,i) - z_pred) * (Z_pred(:,i) - z_pred)'; %calcul de la covariance Pz qui représente l'incertitude associée aux mesures prédites
    end
    
    Pxz = zeros(n, numel(z)); %initialisation d'une matrice vide 
    for i = 1:2*n+1
        Pxz = Pxz + Wc(i) * (X_pred(:,i) - x_pred) * (Z_pred(:,i) - z_pred)'; %corrélation entre l'incertitude des états et l'incertitude des mesures
    end
    
    K = Pxz / Pz; %gain de Kalman
    x_hat = x_pred + K * (z - z_pred);  %met à jour l'estimation des états, l'estimation corrigée des états \uD835\uDC65hat

    P = P_pred - K * Pz * K';      % met à jour la matrice de covariance des états     
    
    
end

% Fonction pour générer les points sigma
function X = sigmaPoints(x, P, gamma)
    n = numel(x);  %dimension de x
    
    % Décomposition de Cholesky de la matrice de covariance P, multipliée par gamma
    A = gamma * chol(P)';  % Calcul de la dispersion des points sigma et transposition
    
    X = zeros(n, 2*n+1);  % Initialisation de la matrice des points sigma
    X(:,1) = x;    % Le premier point sigma est la moyenne des états, x
    for i = 1:n
        X(:,i+1) = x + A(:,i);   % Point sigma i+1 obtenu en ajoutant A(:,i) à x
        X(:,i+n+1) = x - A(:,i);  % Point sigma i+n+1 obtenu en soustrayant A(:,i) de x
    end
end

%%%%%%%%%%%%%%%%%%% Coulomb Counting Method %%%%%%%%%%%%%%%%%%%
function SOC = Coulomb_Counting(courant_decharge, C_N, SoC0)
    % Number of points
    N_points = length(courant_decharge);
    delta_t = 0.2;  % Time step in
    % Initialize the SOC array
    SOC = zeros(1, N_points);  % SoC at each time step
    SOC(1) = SoC0 ;  % Initial SoC in percentage

    % Calculate the state of charge (SOC) for each point with the Coulomb counting method
    for i = 2:N_points
        % Calculate the consumed or added charge in Ah (current * time in seconds)
        charge_consomme = courant_decharge(i) * delta_t;  % Current in A, delta_t in seconds
        
        % Update the SOC according to the Coulomb counting method
        SOC(i) = SOC(i-1) + ((charge_consomme / C_N) * 100);  % Discharge is negative
        if SOC(i) > 100
            SOC(i) = 100;
        elseif SOC(i) < 0
            SOC(i) = 0;
        end
        
        
    end
end