| 查看: 2457 | 回复: 0 | ||
[求助]
无迹卡尔曼滤波迭代过程中状态协方差越来越小，导致状态量更新停滞
|
|
最近在用UKF进行状态参数估计(参照 http://www.ilovematlab.cn/thread-190623-1-1.html 的代码进行修改的) 发现程序迭代几步之后状态协方差就开始越来越小，最终导致状态量无法更新，已经被这个问题困扰一周时间了，一直解决不了，求各位大神指点一下错误的原因 具体程序代码如下： %function data=createst(snr)用于模拟采集到的数据 %function testukf为主函数 %function statenext=systemfun(statelast)为状态方程，由于数据被采集到之后各参数(共a,b,c,d四个参数)即为固定值，所以k+1时刻状态与k时刻状态相同 %function y=measurefun(statelast,time)为测量方程 %function [xc,p]=UKFfiter(systemfun,measurefun,xc0,yc,p0,time)为UKF函数 function data=createst(snr)%用于模拟采集到的数据 %函数模型为s=a*t+b*cos(t)+c*sin(t)+d UKF的目的是根据此处创建的100个(t,s)点，求取对应的a,b,c,d值 a=0.5; b=20; c=10; d=30; t=1:100; for i=1:100 s(1,i)=a*t(1,i)+b*cos(t(1,i))+c*sin(t(1,i))+d; end data(1,  =t;data(2, =awgn(s,snr);end function testukf %------------------清屏---------------- close all;clear all;clc; data=createst(10); t=data(1, ;s=data(2, ;len=length(s); %绘制原始图像 figure plot(t,s,'b','LineWidth',2) global n Rk; %定义全局变量 %------------------初始化-------------- %设定值为 a=0.5; b=20; c=10; d=30; %假设amax=0.7 amin=0.3; bmax=21 bmin=19; cmax=11 cmin=9; dmax=32 dmin=28 state0=[0.6;20.5;9.5;29]; %存在偏差的状态值，用于产生具有偏差的轨迹以及检验滤波效果 %--------系统滤波初始化 Rk=0.05; %状态误差协方差初值 n=4; p=[0.0133 0 0 0;0 0.3333 0 0;0 0 0.3333 0;0 0 0 1.3333];%状态协方差分别为(amax-amin)^2/12 ... %-------------------------------------- xc=state0; xrecord=[]; loopnum=1;%用于设定UKF循环计算的次数 %---------------滤波算法---------------- for j=1:loopnum for i=1:len yc=s(1,i); time=t(1,i); [xc,p]=UKFfiter(@systemfun,@measurefun,xc,yc,p,time); xrecord=[xrecord xc]; end end a=xc(1,1); b=xc(2,1); c=xc(3,1); d=xc(4,1); for i=1:len s1(1,i)=a*i+b*cos(i)+c*sin(i)+d; end %由估计得到的参数绘制的图像 hold on plot(t,s1,'r','LineWidth',2) disp(xc) end function statenext=systemfun(statelast) %状态方程 statenext=eye(4)*statelast;%由于数据一旦被采集到，对应的a,b,c,d即确定下来了，所以迭代过程中k+1时刻的状态量与k时刻状态量相同 end function y=measurefun(statelast,time) %测量方程 a=statelast(1,1); b=statelast(2,1); c=statelast(3,1); d=statelast(4,1); y=a*time+b*cos(time)+c*sin(time)+d; end function [xc,p]=UKFfiter(systemfun,measurefun,xc0,yc,p0,time) global n Rk; %----------------参数注解------------------- % xc0---状态初值(列向量) yc---系统测量值 % p0----状态误差协方差 n----系统状态量数 %systemfun---系统方程 measurefun--测量方程 %---------------滤波初始化------------------ alp=0.5; %default, tunable kap=3-n; %default, tunable beta=2; %default, tunable lamda=alp^2*(n+kap)-n; %scaling factor nc=n+lamda; %scaling factor Wm=[lamda/nc 0.5/nc+zeros(1,2*n)]; %weights for means Wc=Wm; Wc(1)=Wc(1)+(1-alp^2+beta); %weights for covariance ns=sqrt(nc); %------------------------------------------- sxk=0;pxx=0;syk=0;pyy=0;pxy=0; p=p0; %--------------构造sigma点----------------- pk=ns*chol(p); sigma=xc0; for k=1:2*n if(k<=n) sigma=[sigma,xc0+pk(:,k)]; else sigma=[sigma,xc0-pk(:,k-n)]; end end %-------------时间传播方程----------------利用状态方程传递采样点 for ks=1:2*n+1 sigma(:,ks)=systemfun(sigma(:,ks)); sxk=Wm(ks)*sigma(:,ks)+sxk;%x的平均值 end %----------完成对Pxx的估计 for kp=1:2*n+1 pxx=Wc(kp)*(sigma(:,kp)-sxk)*(sigma(:,kp)-sxk)'+pxx; end pxx=pxx+0*eye(4);%0*eye(4)代表的是状态方程中的噪声协方差,Qk %--------------测量更新方程-------------利用sigma点集计算相应的y值 for kg=1:2*n+1 gamma(kg)=measurefun(sigma(:,kg),time); syk=syk+Wm(kg)*gamma(kg);%y的平均值 end %----------完成对Pyy的估计 for kpy=1:2*n+1 pyy=Wc(kpy)*(gamma(kpy)-syk)*(gamma(kpy)-syk)'+pyy; end pyy=pyy+Rk;%0.05代表的是测量方程中的观测噪声协方差,Rk %----------完成对Pxy的估计 for kxy=1:2*n+1 pxy=Wc(kxy)*(sigma(:,kxy)-sxk)*(gamma(kxy)-syk)'+pxy; end kgs=pxy/pyy; %修正系数 xc=sxk+kgs*(yc-syk); %测量信息修正状态 p=pxx-kgs*pyy*kgs'; %误差协方差阵更新 %------------------------------------- end |
回复此楼» 猜你喜欢
溴的反应液脱色
已经有4人回复
-
参与限项
已经有5人回复
-
推荐一本书
已经有7人回复
-
有没有人能给点建议
已经有5人回复
-
假如你的研究生提出不合理要求
已经有12人回复
-
全日制（定向）博士
已经有5人回复
-
萌生出自己或许不适合搞科研的想法，现在跑or等等看？
已经有4人回复
-
Materials Today Chemistry审稿周期
已经有4人回复
-
对氯苯硼酸纯化
已经有3人回复
-
所感
已经有4人回复