无迹卡尔曼滤波估计SOC（附MATLAB程序详解）

设置电流采样周期为1s 

T=1;

 导入电流数据并求电流数据的长度。

I(1,:)=xlsread('UDDS.xlsx')';    
N=length(I);

 设置参数Q为单位矩阵乘以1e-4，设置参数R=1e-5

Q0=1e-4*diag([1,1,1]);
R0=1e-5;

 设置协方差矩阵P0=1e-4乘以单位矩阵；初始化二阶RC模型的参数R0、Rs、Rp、Cs、Cp为一行N列的零矩阵

P0=1e-4*eye(3); 

XAh=zeros(3,N);
XAh(:,1)=[1;0;0];
Z=zeros(1,N); 
AR0_u=zeros(1,N);
ARs=zeros(1,N);
ARp=zeros(1,N);
ACs=zeros(1,N);
ACp=zeros(1,N);

 求解模型参数，通过辨识电池模型参数得到的各个参数与SOC之间的关系用函数表达式表示出来。

    AR0_u(1,t) =-0.3058*XAh(1,t-1)^6+0.9301*XAh(1,t-1)^5-1.036*XAh(1,t-1)^4+0.4936*XAh(1,t-1)^3 ...
         -0.06163*XAh(1,t-1)^2-0.02467*XAh(1,t-1)+0.03971;
    ARs(1,t) = 0.9273*XAh(1,t-1)^6-2.811*XAh(1,t-1)^5+3.328*XAh(1,t-1)^4 ...
         -1.961*XAh(1,t-1)^3+0.6171*XAh(1,t-1)^2-0.1035*XAh(1,t-1)^1+0.0129;
    ARp(1,t) =8.246*XAh(1,t-1)^6-26.15*XAh(1,t-1)^5+32.23*XAh(1,t-1)^4-19.76*XAh(1,t-1)^3 ...
         +6.466*XAh(1,t-1)^2-1.1*XAh(1,t-1)+0.09342;
    ACs(1,t) = -6710*XAh(1,t-1)^6+1.944e+04*XAh(1,t-1)^5 -2.241e+04*XAh(1,t-1)^4 ...
         +1.314e+04*XAh(1,t-1)^3-4231*XAh(1,t-1)^2+772.1*XAh(1,t-1)^1+47.25;
    ACp(1,t) = -1.44e+06*XAh(1,t-1)^7+5.143e+06*XAh(1,t-1)^6-7.211e+06*XAh(1,t-1)^5+4.997e+06*XAh(1,t-1)^4 ...
         -1.757e+06*XAh(1,t-1)^3+2.806e+05*XAh(1,t-1)^2-1.252e+04*XAh(1,t-1)^1+491.1;
    AA=[1,0,0;0,exp(-T/(ARs(1,t)*ACs(1,t))),0;0,0,exp(-T/(ARp(1,t)*ACp(1,t)))];
    AB=[-1/(3*3600);ARs(1,t)*(1-exp(-T/(ARs(1,t)*ACs(1,t))));ARp(1,t)*(1-exp(-T/(ARp(1,t)*ACp(1,t))))];
    XAh(:,t)=AA*XAh(:,t-1)+AB*I(:,t-1);

 OCV与SOC 函数关系，并用6阶多项式拟合。

soc=[1,0.9,0.8,0.7,0.6,0.5,0.4,0.3,0.2,0.1,0];
ocv=[4.1561,4.0832,4.0265,3.9316,3.8368,3.7543,3.6746,3.5959,3.5081,3.3792,3.147];
p1=polyfit(soc,ocv,6);
uoc = zeros(1,N);
for ti=1:N
    uoc(1,ti)=polyval(p1,XAh(1,ti));

 求真实观测电压

Z(1,ti)=uoc(1,ti)-I(1,ti)*AR0_u(1,ti)-XAh(2,ti)-XAh(3,ti)+sqrtm(R0)*randn(1,1);

L为状态维度，a控制采样点的分布状态 1e-4~1，k为待选参数 没有界限，但是要保证（n+lamda）*P为半正定矩阵  状态估计通常为0，b非负的权系数，lambda为缩放比列参数，用于降低总的预测误差，Wm为均值权值，Wc为协方差权值，Wc（1）第一个采样点的均值和协方差的权值，剩下2n个sigma点的均值和协方差的权值，下标m为均值的权值，下标c为协方差的权值

真实SOC与预测SOC，

figure
plot(Xukf(1,:),'-r');
hold on;
plot(XAh(1,:),'--b','linewidth',2);
xlabel('时间/s');
ylabel('SOC');
legend('UKF预测SOC','参考SOC');
end

ukf算法流程

1.无迹变换

function Xukf=ukf(T,I,N,Q0,R0,Z,p1,L,lambda,P0,Wm,Wc,ARs,ARp,ACs,ACp,AR0_u)
Xukf = zeros(3,N);
Xukf(:,1)=[0.8;0;0];    %无迹Kalman滤波状态初始化
for tt=2:N
    xestimate_ukf = Xukf(:,tt-1); 
    P_ukf=P0;
%%%%%%%%%%%%%%%%%%% 1:获得一组Sigma点集 %%%%%%%%%%%%%%
    cho_ukf=(chol(P_ukf*(L+lambda)))';    % cho=chol(X)用于对矩阵X进行Cholesky分解，产生一个上三角阵cho，使cho'cho=X。若X为非对称正定，则输出一个出错信息。
    xgamaP1_ukf=zeros(3,L);
    xgamaP2_ukf=zeros(3,L);
    for k=1:L
        xgamaP1_ukf(:,k)=xestimate_ukf+cho_ukf(:,k);
        xgamaP2_ukf(:,k)=xestimate_ukf-cho_ukf(:,k);
    end
    Xsigma_ukf=[xestimate_ukf xgamaP1_ukf xgamaP2_ukf]; % 获得 2L+1 个Sigma点 

2:对Sigme点集进行一步预测

Xsigmapre_ukf=zeros(3,2*L+1);
   for k=1:2*L+1 
       A_ukf=[1,0,0;0,exp(-T/(ARs(1,tt)*ACs(1,tt))),0;0,0,exp(-T/(ARp(1,tt)*ACp(1,tt)))];
       B_ukf=[-1/(3*3600);ARs(1,tt)*(1-exp(-T/(ARs(1,tt)*ACs(1,tt))));ARp(1,tt)*(1-exp(-T/(ARp(1,tt)*ACp(1,tt))))];
       Xsigmapre_ukf(:,k)=A_ukf*Xsigma_ukf(:,k)+B_ukf*I(:,tt-1);
   end

3：利用预测结果计算均值和协方差

Xpred_ukf=zeros(3,1);
    for k=1:2*L+1
        Xpred_ukf=Xpred_ukf+Wm(k)*Xsigmapre_ukf(:,k);
    end
    Ppred_ukf=zeros(3,3);          %协力差预测均值
    for k=1:2*L+1
        Ppred_ukf=Ppred_ukf+Wc(k)*(Xsigmapre_ukf(:,k)-Xpred_ukf)*(Xsigmapre_ukf(:,k)-Xpred_ukf)';
    end
    Ppred_ukf=Ppred_ukf+Q0;

4:根据预测结果，再次使用UT变换，得到sigma点集

chor_ukf=(chol((L+lambda)*Ppred_ukf))';
     XaugsigmaP1_ukf=zeros(3,L);
     XaugsigmaP2_ukf=zeros(3,L);
    for k=1:L
        XaugsigmaP1_ukf(:,k)=Xpred_ukf+chor_ukf(:,k);
        XaugsigmaP2_ukf(:,k)=Xpred_ukf-chor_ukf(:,k);
    end
    Xaugsigma_ukf=[Xpred_ukf XaugsigmaP1_ukf XaugsigmaP2_ukf];

5:观测预测

Zsigmapre_ukf=zeros(1,2*L+1);
    for k=1:2*L+1
        Zsigmapre_ukf(1,k)=polyval(p1,Xaugsigma_ukf(1,k))-Xaugsigma_ukf(2,k)-Xaugsigma_ukf(3,k)-AR0_u(1,tt-1)*I(1,tt);
    end

6:计算预测观测的均值和协方差

Zpred_ukf=0;
    for k=1:2*L+1
        Zpred_ukf=Zpred_ukf + Wm(k)*Zsigmapre_ukf(1,k);
    end
    Pzz_ukf=0;
    for k=1:2*L+1
        Pzz_ukf=Pzz_ukf+Wc(k)*(Zsigmapre_ukf(1,k)-Zpred_ukf)*(Zsigmapre_ukf(1,k)-Zpred_ukf)';
    end
    Pzz_ukf=Pzz_ukf+R0;
    Pxz_ukf = zeros(3,1);
    for k=1:2*L+1
        Pxz_ukf=Pxz_ukf+Wc(k)*(Xaugsigma_ukf(:,k)-Xpred_ukf)*(Zsigmapre_ukf(1,k)-Zpred_ukf)';
    end

7:计算Kalman增益

K_ukf=Pxz_ukf*inv(Pzz_ukf);  % Kalman

8:状态和方差更新

Xukf(:,tt)=Xpred_ukf+K_ukf*(Z(1,tt)-Zpred_ukf);
    P_ukf=Ppred_ukf-K_ukf*Pzz_ukf*K_ukf';
    P0=P_ukf;   

运行之后的结果如图1所示，可以看出，在SOC的初始值不准确的情况下，无迹卡尔曼滤波算法也能很快的收敛，说明无迹卡尔曼滤波的求解效果还是比较优秀的，在原理上无迹卡尔曼滤波由于采用的无迹变换，而扩展卡尔曼滤波是通过舍去泰勒公式的高阶项，采用无迹卡尔曼滤波算法要优于扩展卡尔曼滤波。

 图1 UKF预测SOC vs 参考SOC值