Untitled

SoC = 0.55; % initial guess SoC, actual initial SoC is approx. 0.9
syms pc nc i x y z;
syms pc32 real;
syms xhat [32 1] real;

% Approximate negative electrode lithium concentration from positive
% electrode concentration
csn = @(z) (theta0_n + ((z - theta0_p*Cmx_p) / ((theta100_p - theta0_p) * Cmx_p)) * (theta100_n - theta0_n));

equilibrium = @(xhat) matCp*(expm(A*Ttime)*xhat); % Takes positive electrode concentrations (nx1)
Equnew = @(xhat)(6/((n+1)*(n+2)*(2*n+3)))*sum((0:n+1).^2*xhat); % Another way to calculate
SoCfunc = @(equilibrium) (equilibrium - conmax_p)/(conmin_p - conmax_p); % SoC from equilibrium

%% Curve fitting instead of lookup table
p1 = 185.1;
p2 = -699.1;
p3 = 1070;
p4 = -853.3;
p5 = 380;
p6 = -93.95;
p7 = 12.79;
p8 = 2.703;
vocvfunc = @(x) p1*(SoCfunc(equilibrium(x)))^7 + p2*(SoCfunc(equilibrium(x)))^6 + ...
                  p3*(SoCfunc(equilibrium(x)))^5 + p4*(SoCfunc(equilibrium(x)))^4 + ...
                  p5*(SoCfunc(equilibrium(x)))^3 + p6*(SoCfunc(equilibrium(x)))^2 + ...
                  p7*(SoCfunc(equilibrium(x))) + p8;

%% Lookup table
matching_vocv_func = @(SoCfunc) vocv(find(abs(socv - SoCfunc) == min(abs(socv - SoCfunc)), 1));

% Voltage of battery is sum of these two, pc is positive electrode lithium
% concentration, VBnew1 needs pc(32) only, only pc(32) is passed to 1 but
% entire PC for VBnew2
VBnew1 = @(pc,i) (gain2*Tt/(alp_n*F))*asinh(i/(2*an*An*Ln*(kn * sqrt(Ce0*(csn(pc)*Cmx_n).*(Cmx_n-(csn(pc)*Cmx_n)))))) + ...
                 (gain2*Tt/(alp_p*F))*asinh((i)/(2*ap*Ap*Lp*(kp * sqrt(Ce0*pc.*(Cmx_p-pc))))) + Rf*i ; % removed
VBnew2 = @(pc) Kgn*(csn(pc(32))-csn(equilibrium(pc)))-Kgp*(pc(32)-equilibrium(pc)) + matching_vocv_func(SoCfunc(equilibrium(pc)));

xhat_sym = sym('xhat', [32, 1]);

%% This section for making partial derivatives of Vocv function, used for lookup table and Equilibrium

% Define symbolic variables
x = sym('x', [32, 1]);

% Define the function vocvfunc
y = SoCfunc(equilibrium(x));

vocvfunc = p1*y^7 + p2*y^6 + p3*y^5 + p4*y^4 + p5*y^3 + p6*y^2 + p7*y + p8;

derivatives = jacobian(vocvfunc, x);
% taken partial derivative of equilibrium terms of voltage equation
equilibrium_sym = Equnew(xhat_sym);

VBnew2_sym = -Kgn * csn(equilibrium_sym) + Kgp * equilibrium_sym;

for idx = 1:32
    dVBnew2_dpc(idx) = diff(VBnew2_sym, xhat_sym(idx));
end
%%

% Partial derivative of terms that only depend on pc(32)
Vb1 = @(pc,i) (gain2*Tt/(alp_n*F))*asinh(i/(2*an*An*Ln*(kn * sqrt(Ce0*(csn(pc)*Cmx_n).*(Cmx_n-(csn(pc)*Cmx_n)))))) + ...
              (gain2*Tt/(alp_p*F))*asinh((i)/(2*ap*Ap*Lp*(kp * sqrt(Ce0*pc.*(Cmx_p-pc))))) + Rf*i ;
Vb2 = @(pc) Kgn*csn(pc)-Kgp*(pc);

dvb1 = diff(Vb1,pc);
dvb2 = diff(Vb2,pc);
%% Constants and initialization

num_steps = 1000; % Number of time steps
dt = 1; % Time step size (1 second)
xp = zeros(length(num_steps),n+2);
xn = zeros(length(num_steps),n+2);
xp(1,:) = (conmax_p - SoC *(conmax_p-conmin_p)) ; %initial conditions
xn(1,:)=conmin_n + SoC *(conmax_n-conmin_n);        %initial conditions
Ttime = 100;    % equilibrium setlling time
ppp = eye(size(matAp))*0.5; %covariance matrix P initial cond

A = matAp; 
B = matBp;

x_hat_history = zeros(length(xp(1,:)), num_steps);
P_history = zeros(length(xp(1,:)), length(xp(1,:)), num_steps);
Equ = zeros(num_steps,1);
SoCeorig = zeros(num_steps,1);
% Initialize state estimate and error covariance
x_hat = xp(1,:)';
P = ppp;
Q = eye(diag(n+2))*2; % Process noise covariance
R = 100;  % Measurement noise covariance


for k = 1:num_steps
    % Define control input and measurement at time step k
    u = i_cell(k); % Control input at time step k
    yy = v_cell(k); % Measurement at time step k


    % Prediction step
    x_hat_pred = x_hat + dt * (A * x_hat + B * -u);

    x_hat_pred_history(k) = x_hat_pred(32);



vestimated(k) = VBnew1(x_hat_pred(32),u) + VBnew2(x_hat_pred);

% All partial derivatives of lookup table in 32x1 matrix
derivative_values_num = double(subs(derivatives, x, x_hat_pred));

%Last jacobian term of state 32
cmatrix(k,1)= double(subs(Vb2,{pc},{x_hat_pred(32)}))+double(subs(Vb1,{pc,i},{x_hat_pred(32),u})) + derivative_values_num(32) ;

%Complete jacobian matrix
    C = [double(dVBnew2_dpc(1:n+1))+derivative_values_num(1:n+1) double(dVBnew2_dpc(n+2))+cmatrix(k,1)];

    %finding kalman gain
    [P_pred,KK,~] = icare(A,C',Q,R);
      K=KK';
 
errr(k) = yy - vestimated(k); %Error output 


x_hat = x_hat_pred +  K * (errr(k))*Cmx_p;


    % Store the state estimate and error covariance etc
    x_hat_history(:, k) = x_hat;
    P_history(:, :, k) = P;
    khistory(:,k) = K;
    csnhistory(k)=csn(x_hat(32));

    %Find equilibrium
    Equ(k)= matCp*(expm(A*Ttime)*x_hat);

    %Find SoC
    SoCeorig(k) = (conmax_p- Equ(k))/(conmax_p-conmin_p);


end