clc
tic
i=tryfilekalmanS1(:,3);
vt=tryfilekalmanS1(:,2);
soc=tryfilekalmanS5(:,1);
t=tryfilekalman(:,1);
OCV=tryfilekalmanS2(:,2);
OCV=table2array(OCV);
current=table2array(i);
voltage=table2array(vt);
SOC=table2array(soc);
Time=table2array(t);
Cn=2.6;
deltat=5;
R=0.1174;
taud=4.4925;
eff=1;
sigmaW =30;
sigmaV=20;
F=(-2.6/taud);A=-2.6;B=1;C=0.1;D=1;
maxIter=16;
M0=277512.69155;
M1=-110959.5326;
M2=14761.90214;
M3=-653.33323;
I = current;
V=voltage;
for k=1:maxIter
xtrue(1)=1;
xhat(1)=0;
sigmaX(1)=0.005;
u=7.7783;
xhat=A*(M0 + M1.*OCV(k) + M2.*OCV(k)^2+ M3.*OCV(k)^3)+w;
sigmaX=A*sigmaX*A'+sigmaW;
u=OCV;
w=0.012; v=0.023;
ytrue = C.*xhat+D.*u+v;
xtrue = A*xtrue+B*u+w;
yhat=C*xhat+D*u;
sigmaY=C*sigmaX*C'+sigmaV;
L=sigmaX*C'/sigmaY;
xhat=xhat+L*(ytrue-yhat);
sigmaX=sigmaX-L*sigmaY*L';
xstore(1:16,k)= xtrue(1:16);
xhatstore(1:16,k)=xhat(1:16);
sigmaXstore(1:1,k)=sigmaX(1:1);
ytruestore(1:17,k)=ytrue(1:17);
yhatstore(1:17,k)=yhat(1:17);
end
toc
figure(1);
plot(1:maxIter,xstore(1:maxIter)','r',1:maxIter,xhatstore',1:maxIter,xhatstore(1:maxIter)-xstore(1:maxIter)','b');
grid;
legend('true','estimate','error');
title('soc estimation');
xlabel('time'); ylabel('soc');