24小时热门版块排行榜    

查看: 2660  |  回复: 0

huazaizju

新虫 (初入文坛)

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

最近在用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
回复此楼
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖
相关版块跳转 我要订阅楼主 huazaizju 的主题更新
最具人气热帖推荐 [查看全部] 作者 回/看 最后发表
[硕博家园] 售SCI文章,我:8O.5.5.1O.54,科目全,可十急 +4 uKOSCocCb6Fm 2026-11-12 10/500 2026-11-16 22:49 by Equinoxhua
[论文投稿] 售一区SCI文章T0P,我:8O.551.O54,科目全,可十急 +4 XJXMuRjD7cK7 2026-11-13 4/200 2026-11-16 11:39 by K7HMGGdOpylw
[硕博家园] 售一区SCI文章T0P,我:8O.551.O54,科目全,可十急 +4 uKOSCocCb6Fm 2026-11-13 4/200 2026-11-16 11:06 by K7HMGGdOpylw
[公派出国] 售一区SCI文章T0P,我:8O.551.O54,科目全,可十急 +3 SX5mJmPWDfOO 2026-11-13 3/150 2026-11-15 08:29 by xSrQNSSjcgoX
[论文投稿] 售SCI一区T0P文章,我:8O.55.1.O.5.4,科目齐全,可+急 +4 SX5mJmPWDfOO 2026-11-13 4/200 2026-11-15 07:45 by xSrQNSSjcgoX
[论文投稿] 售SCI-T0P文章,我:8O.5.5.1.O.54,科目齐全,可+急 +4 uKOSCocCb6Fm 2026-11-12 7/350 2026-11-15 03:10 by xSrQNSSjcgoX
[考研] 售SCI一区文章,我:8O5.5.1.O5.4,科目全,可伽急 +5 G2rhSvqvhNzl 2026-11-10 7/350 2026-11-14 21:58 by Equinoxhua
[教师之家] 售SCI一区T0P文章,我:8.O.55.1.O.5.4,科目全,可+急 +4 1Nb0IVlmV6v0 2026-11-11 4/200 2026-11-13 11:18 by Equinoxhua
[考研] 售SCI一区文章,我:8.O.551.O.5.4,科目全,可伽急 +5 C6CRn66RYYXS 2026-11-10 6/300 2026-11-13 11:04 by Equinoxhua
[考博] 27博士申请(市政/环境) +3 ladf 2026-11-12 3/150 2026-11-13 08:19 by Equinoxhua
[考研] 售SCI一区T0P文章,我:8.O.55.1.O.5.4,科目全,可+急 +4 Z7qfCsdIiD1T 2026-11-11 5/250 2026-11-12 11:44 by HajghKOMynmz
[公派出国] 售SCI一区T0P文章,我:8.O.55.1.O.54,科目齐全,可+急 +3 Z7qfCsdIiD1T 2026-11-10 7/350 2026-11-12 11:31 by HajghKOMynmz
[考博] 售SCI一区T0P文章,我:8O.55.1.O.5.4,科目齐全,可+急 +4 czAiolzR0IJC 2026-11-10 7/350 2026-11-12 11:11 by HajghKOMynmz
[教师之家] 售SCI一区T0P文章,我:8.O.55.1.O54,科目全,可伽急 +5 czAiolzR0IJC 2026-11-10 7/350 2026-11-12 11:09 by HajghKOMynmz
[教师之家] 售SCI-T0P文章,我:8O.5.5.1.O.54,科目齐全,可+急 +4 czAiolzR0IJC 2026-11-10 7/350 2026-11-12 11:00 by HajghKOMynmz
[考研] 售SCI一区T0P文章,我:8O.55.1.O.54,科目全,可伽急 +4 G2rhSvqvhNzl 2026-11-10 7/350 2026-11-12 10:49 by HajghKOMynmz
[考研] 售SCI一区T0P文章,我:8.O.55.1.O.5.4,科目全,可+急 +5 C6CRn66RYYXS 2026-11-10 7/350 2026-11-12 10:16 by HajghKOMynmz
[硕博家园] 售SCI一区文章,我:8.O.55.1.O.54,科目齐全,可伽急 +4 C6CRn66RYYXS 2026-11-10 6/300 2026-11-12 10:16 by HajghKOMynmz
[公派出国] 售SCI-T0P文章,我:8O.5.5.1.O.54,科目齐全,可+急 +5 YHiw1gou1Eda 2026-11-10 6/300 2026-11-12 10:14 by HajghKOMynmz
[教师之家] 售SCI一区T0P文章,我:8.O.55.1.O.54,科目齐全,可+急 +3 TgvkbZZl9tqB 2026-11-11 4/200 2026-11-12 08:15 by HajghKOMynmz
信息提示
请填处理意见