本帖最后由 华仔zju 于 -3-23 16:40 编辑
最近项目上在利用卡尔曼滤波进行模型参数估计，编写的EKF程序运行良好，后来看到论文提及UKF效果会比EKF更好，便想着使用UKF试一试
仿造 /thread-190623-1-1.html 编写代码，运行之后发现仅在前几步迭代过程中状态会更新，随后状态协方差会变得很小，导致状态量几乎不变化。
以下是简化版的代码，求各位大神指点迷津
% function data=createst(snr)为信号发生函数
% function testukf为主函数
% functionstatenext=systemfun(statelast)为状态方程，一旦数据采集得到，那么模型中状态量就固定了，此处认为k+1时的状态与k时状态相同
% functiony=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根据给定的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(50);%时间τ在48us
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;
state0=[0.8;21;8;28];%存在偏差的状态值，用于产生具有偏差的轨迹以及检验滤波效果
%--------系统滤波初始化
Rk=0.05;%状态误差协方差初值
n=4;
p=[0.025 0 0 0;0 4 0 0;0 0 25 0;0 0 0 16];
%--------------------------------------
xc=state0;
xrecord=[];
loopnum=1;
%---------------滤波算法----------------
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
functionstatenext=systemfun(statelast)
%系统方程
statenext=eye(4)*statelast;
end
functiony=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
如果觉得《ukf实测信号的预测 matlab 无迹卡尔曼滤波(UKF)在参数估计应用中迭代停滞问题》对你有帮助，请点赞、收藏，并留下你的观点哦！
