无迹卡尔曼滤波UKF_代码及调参(2)

  版权声明:本文为博主原创文章,未经博主允许不得转载。https://blog.csdn.net/weixin_38451800/article/details/85721991

从试验工况到建模离散化,再到模型参数辨识,再到最简单的卡尔曼,比如扩展卡尔曼和自适应卡尔曼,接下来,我们如果专注于算法方面的话,自然到Sigma点滤波,这其中包括前几篇博客中我就提到的无迹卡尔曼UKF,中心差分卡尔曼CDKF,容积卡尔曼CKF以及粒子滤波PF,粒子滤波某种程度上也可以看成一种Sigma点滤波器。
  再然后呢,当然就到了神经网络(BP,RBF或者Hopfield等)和模糊控制。很不幸的是,到BP前除了我先前说的CKF博主不做,其他都做过。这之间的辛苦当然别人是无法理解的喜悦同样是。好了,回到正题,本小节接着上节开始谈无迹卡尔曼滤波UKF
备注:这里面还有几本测试手册,比如USABC,PNGV,QCT897-2011等,有人需要的话,你可以自行网上下载,也可以留言我看到会分享的;还有一些测试设备方面我也可以开博客介绍一下,毕竟这边好多报价单了,就是贵的都买不起,尴尬)

1.前言
  博主个人建议无迹卡尔曼学习方面还是上节提到的黄小平老师的《卡尔曼滤波原理及应用-MATLAB仿真》,另外加上几篇优秀硕博毕业论文就好。所以,本小节就不一一介绍无迹卡尔曼滤波的原理和具体实施步骤了。

2.编程
  编程方面,博主会附上自己的无迹卡尔曼Matlab源代码,主要应用于二阶等效模型锂离子电池SOC估算,其中总的框架你们直接用就好了,实验数据要带入你们自己的,然后改模型初始值以及一些对应参数便可。但是博主还是舍不得,毕竟有师弟要交换或者是money我都没有同意(都是自己手敲的,应用面就这么窄觉得不值得),放在CSDN上面也是一笔不少的C币收入。还是算了吧,放个基本的UKF的吧。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  无迹Kalman滤波(UKF)在锂电池SOC估计中的应用
%  等效电路模型:二阶等效电路模型
%  递推最小二乘法参数识别
%  编程人:a往南向北,日期:2017/08/11
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;close all;clc;
%1.1 加载放电电流电压数据
load consu11;              %带入充放电电流数据
load voltage11;            %带入对应的电压数据
u=consu2;
vol=voltage2;
%1.2 拟合出ocv-soc曲线关系
soc=;
uoc=;
p1=polyfit(soc,uoc,6);
%%2 定义一些初始变量
T=;                                          %采样时间
a=1;                                         %电流系数
b=1;                                         %温度系数
q=;                                          %电池容量(安秒)
qr=1;                                        %电池容量系数
N=;                                          %总的采样次数
X=zeros(3,N);                                % 目标真实值
X(:,1)=[,,];                                 %目标初始值,取soc0=1,uc0=0;
Z=zeros(1,N);                                %观测值即端电压值
delta_w=1e-6;                                %如果增大这个参数,目标真实轨迹就是曲线
Q=delta_w*[1,0,0;
0,1,0;0,0,1];                                %过程噪音均值
R=0.01;                                      %观测噪音方差
%2.1 载入最小二乘法辨识的电阻和电容值
r0=;
r1=;
r2=;
c1=;
c2=;
%2.2 确定状态方程及其中矩阵
A=[1,0,0;
0,exp(-T/r1/c1),0;0,0,exp(-T/r2/c2)];                             %状态转移矩阵
B=[-1*a*b*T/q/qr;
r1*(1-exp(-T/r1/c1));r2*(1-exp(-T/r2/c2))];                       %过程噪音驱动矩阵                 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%3 画出目标真实轨迹曲线
v=sqrtm(R)*randn(1,N);
for t=2:N
    X(:,t)=A*X(:,t-1)+B*u(t-1)+sqrtm(Q)*randn(3,1);               %安时积分法估计soc的曲线  
end
for t=1:N
    soc(t)=X(1,t);                            
    uoc(t)=polyval(p1,soc(t));
    Z(t)=uoc(t)-r0*u(t)-X(2,t)-X(3,t)+v(t);                       %对目标的观测
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%4 UKF滤波算法,UT变换
L=3;                                          %状态向量维数
alpha=0.01;                                   %控制采样点分布状态
kalpha=0;                                     %待选参数,确保半正定
belta=2;                                      %非负权系数,合并高阶项动差,把高阶项影响包括
ramda=3*alpha^2-L;                            %缩放比例参数,降低总的预测误差
for j=1:2*L+1
    Wm(j)=1/(2*(L+ramda));
    Wc(j)=1/(2*(L+ramda));
end
Wm(1)=ramda/(L+ramda);                         %均值权值计算
Wc(1)=1/(L+ramda)+1-alpha^2+belta;             %方差权值计算
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xukf=zeros(3,N);
Xukf(:,1)=X(:,1);                              %无迹Kalman滤波状态初始化
P0=1e-2*[1,0,0;
0,1,0;0,0,1];                                  %协方差阵初始化
for t=2:N
    xestimate= Xukf(:,t-1);
    P=P0;
    
    %第一步:获得一组Sigmma点集
    cho=(chol(P*(L+ramda)))';                  %chol用于对矩阵进行cholesky分解
    for k=1:L
        xgamaP1(:,k)=xestimate+cho(:,k);
        xgamaP2(:,k)=xestimate-cho(:,k);
    end
    Xsigma=[xestimate,xgamaP1,xgamaP2];         %Sigma点集
   
    %第二步:对Sigma点集进行一步预测
    for k=1:2*L+1
        Xsigmapre(:,k)=A*Xsigma(:,k)+B*u(t-1);    %t-1时刻,状态方程预测
    end
    
    %第三步:利用第二步的结果计算均值和协方差
    Xpred=zeros(3,1);                             %均值
    for k=1:2*L+1
        Xpred=Xpred+Wm(k)*Xsigmapre(:,k);
    end
    Ppred=zeros(3,3);                             %协方差阵预测
    for k=1:2*L+1
 Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';
    end
    Ppred=Ppred+Q;        
    
    %第四步:根据预测值,再一次使用UT变换,得到新的Sigma点集
    chor=(chol((L+ramda)*Ppred))';
    for k=1:L
        XaugsigmaP1(:,k)=Xpred+chor(:,k);
        XaugsigmaP2(:,k)=Xpred-chor(:,k);
    end
    Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
    
    %第五步:观测预测
    for k=1:2*L+1                                  %观测预测
        soc=Xaugsigma(1,k);
        Zsigmapre(1,k)=polyval(p1,soc)-Xaugsigma(2,k)-Xaugsigma(3,k)-r0*u(t);
    end
    
    %第六步:计算观测预测均值和协方差
    Zpred=0;
    for k=1:2*L+1
        Zpred=Zpred+Wm(k)*Zsigmapre(1,k);
    end
    Pzz=0;
    for k=1:2*L+1
Pzz=Pzz+Wc(k)*(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)';
    end
    Pzz=Pzz+R;
    Pxz=zeros(3,1);
    for k=1:2*L+1
Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)';
    end 
    Zukf(1,1)=Z(1,1);
    Zukf(1,t)=Zpred;
    %第七步:计算卡尔曼增益
    K=Pxz*inv(Pzz);
    %第八步:状态和方差更新
    xestimate=Xpred+K*(vol(t)-Zpred);
    P=Ppred-K*Pzz*K';
    P0=P;
    Xukf(:,t)=xestimate;
    %%%5安时积分法估计soc
    socAH(1)=X(1,1)-u(1)*T/q;
    socAH(t)=socAH(t-1)-u(t)*T/q;
end             
%%5 误差分析
for i=1:N
    Err_KalmanFilter(1,i)=socAH(i)-Xukf(1,i);
end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%6 画图略

3.调参
调参,我在上节中就已提到,从算法的角度来看,分两个方面:(1)算法的初始参数,比如初值SOC、初始协方差、Q和R等;(2)模型等效模型输入方式,目前主要有两种方式,一种是拟合关系的形式,一种是分段代入的形式,还有的是ocv-soc拟合关系精度问题;(3)当然最后一个是最重要的就是UT变换中参数怎么定问题,很多人甚至都出现了协方差非半正定问题,这个解决方法是有的,而且挺多,经验运气成分占比都有一些。
无迹卡尔曼滤波UKF_代码及调参(2)_第1张图片
以上是我自己算法中,给出的一种参考值,每个参数的具体意义我也都具体标明。这个真的靠经验,你要尝试找规律,多尝试,感觉不能再说了,哈哈哈,具体问题咱在具体分析吧。

你可能感兴趣的:(电池管理系统BMS,无迹卡尔曼UKF,Matlab代码,锂电池,二阶等效模型,编程调参)