UKF-MATLAB实现

最新推荐文章于 2024-07-10 09:48:07 发布

&~&

最新推荐文章于 2024-07-10 09:48:07 发布

阅读量5.4k

点赞数 4

分类专栏：机器学习文章标签： matlab 机器学习

本文链接：https://blog.csdn.net/LJP1924804579/article/details/105917965

版权

机器学习专栏收录该内容

4 篇文章 1 订阅

订阅专栏

记一次作业：
在这里插入图片描述

在题目要求条件下的拟合曲线图：

拟合效果并不是太好，这个图已经是我感觉拟合效果最好的图了，而且并不稳定，可以说你再运行一次并不一定得到这个图，后面把 f（x）函数中的25改成2.5之后，普遍得到的拟合效果都挺不错。
下面两图就是把值改为2.5之后的图：
在这里插入图片描述

参考链接：
https://max.book118.com/html/2017/0502/103920556.shtm
https://max.book118.com/html/2017/0502/103920556.shtm
附上代码：

function UKF
global L Q R;
N=50;%次数
L=1;
Q=10;
R=1;
sum = 0;%计算拟合率
W=sqrt(Q)*randn(L,N);
V=sqrt(R)*randn(L,N);
X=zeros(L,N);
X(:,1)=[0.1]';
Z=zeros(1,N);
Z(1)=X(:,1)^2/20+V(1);
Xukf=zeros(L,N);
Xukf(:,1)=X(:,1)+sqrtm(Q)*randn(L,1);%初始值
Pukf=eye(L);
err_ukf=zeros(1,N);%误差
for k=2:N
    X(:,k)=0.5*X(:,k-1)+2.5*X(:,k-1)/(1+X(:,k-1)^2)+8*cos(1.2*k)+W(k);
    Z(k)=X(:,k)^2/20+V(k);
    [Xukf(:,k),Pukf]=filter(Xukf(:,k-1),Pukf,Z(k),k);
end

for k=1:N
    err_ukf(k)=abs(Xukf(1,k)-X(1,k));
    if err_ukf(k) < 3
        sum = sum + 1;
    end
end
percent = sum / N

figure
hold on;box on;
plot(X,'-r*');
plot(Xukf,'-b+');
legend('真实状态','UKF估计')
xlabel('时间k/s')
ylabel('状态值')

figure
hold on;box on;
plot(err_ukf,'-b*');
xlabel('时间k/s')
ylabel('偏差绝对值')
legend('误差')
%---------------滤波--------------------
function [xk,pk]=filter(X0,P0,y,k)
global L Q R;
alpha=1;
belta=2;
ramda=3-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)=ramda/(L+ramda)+1-alpha^2+belta;
sxk = 0;spk = 0;syk = 0;
py = 0;pxy = 0;
P=P0;
cho=(chol(P*(L+ramda)))';
%-----------构造sigma点------------
for j=1:L
    xsigma1(:,j)=X0+cho(:,j);
    xsigma2(:,j)=X0-cho(:,j);
end
xsigma=[X0,xsigma1,xsigma2];
%------------时间传播方程--------------
for j=1:2*L+1
    xsigma(:,j)=0.5*xsigma(:,j)+2.5*xsigma(:,j)/(1+xsigma(:,j)^2)+8*cos(1.2*k);
    sxk=sxk+Wm(j)*xsigma(:,j);
end
%-------------measure----------
for j=1:2*L+1
    spk=spk+Wc(j)*(xsigma(:,j)-sxk)*(xsigma(:,j)-sxk)';
end
spk=spk+Q;
%--------system-------
for j=1:2*L+1
    rk(1,j)=xsigma(:,j)^2/20;
end
for j=1:2*L+1
    syk=syk+Wm(j)*rk(1,j);
end
%-------------测量更新方程----------------
for j=1:2*L+1
    py=py+Wc(j)*(rk(1,j)-syk)*(rk(1,j)-syk)';
end
py=py+R;
for j=1:2*L+1
    pxy=pxy+Wc(j)*(xsigma(:,j)-sxk)*(rk(1,j)-syk)';
end
K=pxy*inv(py);
xk=sxk+K*(y-syk);
pk=spk-K*py*K';