无迹卡尔曼滤波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.编程
　　编程方面，博主会附上自己的无迹卡尔曼Ｍatlab源代码，主要应用于二阶等效模型锂离子电池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变换中参数怎么定问题，很多人甚至都出现了协方差非半正定问题，这个解决方法是有的，而且挺多，经验运气成分占比都有一些。

以上是我自己算法中，给出的一种参考值，每个参数的具体意义我也都具体标明。这个真的靠经验，你要尝试找规律，多尝试，感觉不能再说了，哈哈哈，具体问题咱在具体分析吧。