基于无迹卡尔曼滤波的SOC估算_卡尔曼滤波_锂电池SOC估算模型_SOC估算模型_matlab仿真

%零状态滞后模型 %AUKF进行SOC的估算 function test_SimpleModel_SOC_UKF(block) setup(block); %endfunction function setup(block) % Register number of ports block.NumInputPorts = 2; block.NumOutputPorts = 3; % Setup port properties to be inherited or dynamic block.SetPreCompInpPortInfoToDynamic; block.SetPreCompOutPortInfoToDynamic; % Override input port properties block.InputPort(1).Dimensions = 3; %系统输入yk, ik,ik-1 block.InputPort(1).DatatypeID = 0; % double block.InputPort(1).SamplingMode = 'Sample'; block.InputPort(1).Complexity = 'Real'; block.InputPort(1).DirectFeedthrough = true; % block.InputPort(2).Dimensions = 1; %clock 时钟输入 block.InputPort(2).DatatypeID = 0; % double block.InputPort(2).SamplingMode = 'Sample'; block.InputPort(2).Complexity = 'Real'; block.InputPort(2).DirectFeedthrough = false; % Override output port properties block.OutputPort(1).Dimensions = 1; %状态输出zk block.OutputPort(1).DatatypeID = 0; % double block.OutputPort(1).SamplingMode = 'Sample'; block.OutputPort(1).Complexity = 'Real'; block.OutputPort(2).Dimensions = 1; %状态向量误差协方差输出Pxk block.OutputPort(2).DatatypeID = 0; % double block.OutputPort(2).SamplingMode = 'Sample'; block.OutputPort(2).Complexity = 'Real'; block.OutputPort(3).Dimensions = 2; %噪声协方差 block.OutputPort(3).DatatypeID = 0; % double block.OutputPort(3).SamplingMode = 'Sample'; block.OutputPort(3).Complexity = 'Real'; % Register parameters block.NumDialogPrms = 1; %没有手动输入的参数 block.SampleTimes = [-1 0]; block.SimStateCompliance = 'DefaultSimState'; block.RegBlockMethod('PostPropagationSetup', @DoPostPropSetup); %定义状态变量 block.RegBlockMethod('Start', @Start); block.RegBlockMethod('Outputs', @Outputs); % Required function DoPostPropSetup(block)%定义状态变量 block.NumDworks = 5; block.Dwork(1).Name = 'x1'; %表示状态zk block.Dwork(1).Dimensions = 1; block.Dwork(1).DatatypeID = 0; % double block.Dwork(1).Complexity = 'Real'; % real block.Dwork(1).UsedAsDiscState = true; block.Dwork(2).Name = 'p1';%表示误差协方差pk block.Dwork(2).Dimensions = 1; block.Dwork(2).DatatypeID = 0; % double block.Dwork(2).Complexity = 'Real'; % real block.Dwork(2).UsedAsDiscState = true; % block.Dwork(3).Name = 'QR';%表示噪声协方差 block.Dwork(3).Dimensions = 2; block.Dwork(3).DatatypeID = 0; % double block.Dwork(3).Complexity = 'Real'; % real block.Dwork(3).UsedAsDiscState = true; block.Dwork(4).Name = 'Un';%自适应噪声协方差调整中使用的存储数据 block.Dwork(4).Dimensions = 11; block.Dwork(4).DatatypeID = 0; % double block.Dwork(4).Complexity = 'Real'; % real block.Dwork(4).UsedAsDiscState = true; block.Dwork(5).Name = 'sk';%考虑迟滞模型的参数 block.Dwork(5).Dimensions = 1; block.Dwork(5).DatatypeID = 0; % double block.Dwork(5).Complexity = 'Real'; % real block.Dwork(5).UsedAsDiscState = true; % end function function Start(block) %初始化函数 block.Dwork(1).Data =[0.5]; %状态zk block.Dwork(2).Data=[0.05]; %误差协方差p block.Dwork(3).Data=[0.2 0.2];%噪声协方差 block.Dwork(4).Data=zeros(1,11);%自适应需要存储的数据 block.Dwork(5).Data=0;%sk % endfunction function Outputs(block) %更新状态，并输出函数 sampletime=block.DialogPrm(1).Data; x=block.Dwork(1).Data; %状态参数设为x p=(block.Dwork(2).Data)'; %误差协方差矩阵为实数 u=(block.InputPort(1).Data)'; %输入向量，yk，ik,ik-1 QR=block.Dwork(3).Data; %存储过程噪声和测量噪声协方差 Un=block.Dwork(4).Data; %所需用到的输出与根据估计状态计算出的输出的差的序列 sk=block.Dwork(5).Data; %存储前一个时间的sk if block.InputPort(2).Data>=20 end if block.InputPort(2).Data>=20 pr_chol=QR(1); %过程噪声误差协方差 pe_chol=QR(2); %测量噪声的协方差。 %利用UKF中需要先取值alpha，bet，这里取bet=2 L=1; alpha=0.01; h=alpha*(3^.5); alpham_1=1-(L/(3*alpha^2)); %权重m在i等于1的时候的值 alphac_1=4-(L/(3*alpha^2))-alpha^2; %权重c在i等于1的时候的值 alpham=1/(6*alpha^2); %权重在i不等于1的时候的值 %误差协方差的乔治分解，分解成下三角，运用自己编写的chol_1分解能够实现正定与半正定矩阵的分解 p_chol=h*chol_1(p); %建立sigmak-1|k-1点 sigmapoint=zeros(1,3); sigmapoint(1)=x; sigmapoint(2)=x+p_chol; sigmapoint(3)=x-p_chol; %更新sigma*k|k-1点 for i=1:3 sigmapoint(i)=sigmapoint(i)-(sampletime/(7.5*3600))*u(3); end %时间更新zk x=alpham_1*sigmapoint(1)+alpham*(sigmapoint(2)+sigmapoint(3)); %时间更新p p=0; for i=1:3 if i==1 p=alphac_1*(sigmapoint(i)-x)*(sigmapoint(i)-x)'; else p=p+alpham*(sigmapoint(i)-x)*(sigmapoint(i)-x)'; end end p=p+pr_chol; %再次更新sigmapointk|k-1 sigmapoint=zeros(1,3); sigmapoint(1)=x; sigmapoint(2)=x+h*chol_1(pr_chol); sigmapoint(3)=x-h*chol_1(pr_chol); %根据ik的大小更新sk if u(2)<-0.1 sk=-1; elseif u(2)>0.1 sk=1; end %计算输出估计的sigmapoint点 yy=zeros(1,3); for i=1:3 if u(2)>=0 yy(i)=4.3-0.0097*u(2)-0.0123/sigmapoint(i)-0.2531*sigmapoint(i)+0.1225*log(sigmapoint(i))-0.0456*log(1-sigmapoint(i)); else yy(i)=4.3-0.0090*u(2)-0.0123/sigmapoint(i)-0.2531*sigmapoint(i)+0.1225*log(sigmapoint(i))-0.0456*log(1-sigmapoint(i)); end end %计算输出估计 yk=0; for i=1:3; if i==1 yk=alpham_1*yy(i); else yk=yk+alpham*yy(i); end end %计算输出估计的误差协方差 pyk=0; for i=1:3 if i==1; pyk=pyk+alphac_1*(yy(i)-yk)*((yy(i)-yk)'); else pyk=pyk+alpham*(yy(i)-yk)*((yy(i)-yk)'); end end pyk=pyk+pe_chol; %计算与状态和输出估计均有关的误差协方差 pxyk=0; for i=1:3 if i==1; pxyk=pxyk+alphac_1*(sigmapoint(i)-x)*((yy(i)-yk)'); else pxyk=pxyk+alpham*(sigmapoint(i)-x)*((yy(i)-yk)'); end end % if pyk<1e-30 || pxyk<1e-30 % Lk=0; % else % % end Lk=pxyk/pyk; %测量更新xk x=x+Lk*(u(1)-yk); %测量更新p p=p-Lk*pyk*(Lk'); %进行自适应所需的计算 Un(1:9)=Un(2:10); if u(2)>=0 Un(10)=u(1)-(4.3-0.0097*u(2)-0.0123/x-0.2531*x+0.1225*log(x)-0.0456*log(1-x)); else Un(10)=u(1)-(4.3-0.0097*u(2)-0.0123/x-0.2531*x+0.1225*log(x)-0.0456*log(1-x)); end Un(11)=Un(11)+1; %更新Q和R，噪声误差协方差 if Un(11)>=10 Un_1=Un(1:10).*Un(1:10); Fk=sum(Un_1)/10; QR(1)=Lk*Fk*Lk'; QR(2)=0; for i=1:3; if i==1 QR(2)=alphac_1*(yy(i)-u(1)+Un(10))*(yy(i)-u(1)+Un(10))'; else QR(2)=QR(2)+alpham*(yy(i)-u(1)+Un(10))*(yy(i)-u(1)+Un(10))'; end end QR(2)=Fk+QR(2); end %更新函数中的状态，作为k-1|k-1 block.Dwork(1).Data=x; block.Dwork(2).Data=p; block.Dwork(3).Data=QR; block.Dwork(4).Data=Un; block.Dwork(5).Data=sk; end %输出 block.OutputPort(1).Data = block.Dwork(1).Data ; block.OutputPort(2).Data = block.Dwork(2).Data ; block.OutputPort(3).Data = block.Dwork(3).Data ; %end Outputs