%% EKF
T=1;%采样时间为1s
N=20042/T;%总步长
x=zeros(3,N);%定义状态向量x,定义一个3行N列的零矩阵。
x(:,1)=[0;0;1];%状态向量x初值设定
global P; %global 声明全局变量
P=eye(3);%定义协方差 EKF,生成一个3行3列的单位矩阵
Q=0.1;R=0.1;
global Q_1;
%%
%ii(1,:)=I;uu(1,:)=Ub;soc(1,:)=SOC;%输入参数
i = xlsread('i.xlsx'); %读取电流数据
u = xlsread('v.xlsx'); %读取电压数据
soc1 = xlsread('soc.xlsx'); %读取soc数据
ii = i'; %将电流数据变成行向量
uu = u'; %将电压数据变成行向量
soc = soc1'; %将soc数据变成行向量
I = ii(1,:);Ub = uu(1,:);SOC = soc(1,:); %访问数组第一行的所有元素？？？
Cn=33.271;%电池容量
%%
%通过数据拟合方法，基于Matlab的曲线拟合工具箱cftool结合hppc辨识出来的参数。
for i=2:N
R1=0.03023*x(3,i-1)^6-0.1141*x(3,i-1)^5+0.1689*x(3,i-1)^4-0.1243*x(3,i-1)^3+0.04728*x(3,i-1)^2-0.008527*x(3,i-1)+0.0006967;
R2=0.003971*x(3,i-1)^6-0.005341*x(3,i-1)^5-0.006872*x(3,i-1)^4+0.01435*x(3,i-1)^3-0.007282*x(3,i-1)^2+0.001231*x(3,i-1)+0.0002082;
C1=510100*x(3,i-1)^6-1276000*x(3,i-1)^5+1031000*x(3,i-1)^3-176300*x(3,i-1)^2+69310*x(3,i-1)+909;
C2=-12940000*x(3,i-1)^6+39640000*x(3,i-1)^5-45050000*x(3,i-1)^4+23320000*x(3,i-1)^3-5467000*x(3,i-1)^2+523200*x(3,i-1)+52340;
A=[1-(1/(R1*C1)) 0 0;0 1-(1/(R2*C2)) 0;0 0 1];%系数矩阵A 3行3列
B=[1/C1 1/C2 1/(3600*Cn)]';%系数矩阵B 3行1列
H=(66.48+54.18*ii(1,i-1))*x(3,i-1)^5-(127.9+196.8*ii(1,i-1))*x(3,i-1)^4+(70.16+262.88*ii(1,i-1))*x(3,i-1)^3-(3.48-163.38*ii(1,i-1))*x(3,i-1)^2....
-(4.58-48.42*ii(1,i-1))*x(3,i-1)-5.77*ii(1,i-1)+1.26; %求一阶泰勒近似
C=[1,1,H];%系数矩阵C 1行3列
if ii(1,:)>=0 %R0区分充放电来拟合，也是关于SOC的函数
R0=(-0.1313*x(3,i-1)^3+0.4812*x(3,i-1)^2-0.5452*x(3,i-1)+2.96)/1000;%充电时欧姆内阻三阶拟合,除1000表示毫Ω化Ω
else
R0=(9.033*x(3,i-1)^6-39.36*x(3,i-1)^5+65.72*x(3,i-1)^4-54.46*x(3,i-1)^3+24.21*x(3,i-1)^2 -5.774*x(3,i-1)+2.58)/1000;%放电时欧姆内阻6阶拟合 除1000表示毫Ω化Ω
end
Ppre=A*P*A'+eye(3)*Q;%协方差预测更新 3行3列
xpre(:,i-1)=A*x(:,i-1)+B*ii(1,i-1);%状态x的预测更新 3行1列 第一行为U1,；第二行为U2；第三行为SOC
Usoc=11.08*xpre(3,i-1)^6-25.58*xpre(3,i-1)^5+17.54*xpre(3,i-1)^4-1.159*xpre(3,i-1)^3-2.386*xpre(3,i-1)^2....
+1.263*xpre(3,i-1)+3.422;%开路电压Usoc是关于SOC的函数
K(:,i-1)=Ppre*C'*inv(C*Ppre*C'+R);%卡尔曼增益更新
Um(1,i-1)=Usoc+xpre(1,i-1)+xpre(2,i-1)+ii(1,i-1)*R0;%预测的电池端电压
e(1,i-1)=uu(1,i-1)-Um(1,i-1);%新息
x(:,i)=xpre(:,i-1)+K(:,i-1)*e(1,i-1);%状态的后验估计 / 状态估计测量更新
P=(eye(3)-K(:,i-1)*C)*Ppre;%协方差更新
end
figure
hold on;box on;
plot(x(3,:),'r','Linewidth',5);
plot(soc(1,:),'b','Linewidth',5);
xlabel('time s');
ylabel('SOC');
figure
hold on;box on;
plot(soc(1,:)-x(3,:),'c','Linewidth',3);
