24小时热门版块排行榜

返回列表

huazaizju

新虫 (初入文坛)

应助: 0 (幼儿园)
金币: 11
帖子: 1
在线: 1.5小时
虫号: 3756302
注册: 2015-03-22

[求助] 无迹卡尔曼滤波迭代过程中状态协方差越来越小，导致状态量更新停滞

最近在用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

回复此楼

» 猜你喜欢

1楼 2017-03-24 10:58:50

已阅回复此楼关注TA 给TA发消息送TA红花 TA的回帖

相关版块跳转我要订阅楼主 huazaizju 的主题更新

返回列表