%{
EKF UKF PF 对比程序
%}
clear all
close all
clc
N=500;
T=1;
w=0.01;
F=[1 sin(w)/w 0 (1-cos(w))/w;
0 cos(w) 0 -sin(w);
0 (1-cos(w))/w 1 sin(w)/w;
0 sin(w) 0 cos(w)];
W=[0.5 0;
1 0;
0 0.5;
0 1];
% %%参数调节区
rl=0.5;
P=[100 0 0 0;
0 0.01 0 0;
0 0 100 0;
0 0 0 0.01];
Qc=0.01*0.01*eye(2);
rtheta=0.02;
R=[rl^2 0;0 rtheta^2];
x=zeros(4,N);
x(:,1)=[100,1,100,1]';
l=zeros(1,N);
theta=zeros(1,N);
z=zeros(2,N);
for i=2:N
x(:,i)=F*x(:,i-1);
end
for i=1:N
xx(:,i)=x(:,i)+W*(sqrt(Qc)*randn(2,1));
l(i)=sqrt(xx(1,i)^2+xx(3,i)^2)+rl*randn;
theta(i)=atan2(xx(3,i),xx(1,i))+rtheta*randn;
z(1,i)=l(i)*cos(theta(i));
z(2,i)=l(i)*sin(theta(i));
end
mekf=0;
mukf=0;
mpf=0;
tic;
E_xx=zeros(4,N);
E_xx(:,1)=x(:,1);
%EKF
for i=2:N
E_xx(:,i)=F*E_xx(:,i-1);
P_p=F*P*F'+W*Qc*W';
h=[ sqrt(E_xx(1,i)^2+E_xx(3,i)^2);
atan2(E_xx(3,i),E_xx(1,i))];
H=[ E_xx(1,i)/sqrt(E_xx(1,i)^2+E_xx(3,i)^2) 0 E_xx(3,i)/sqrt(E_xx(1,i)^2+E_xx(3,i)^2) 0;
-E_xx(3,i)/(E_xx(1,i)^2+E_xx(3,i)^2) 0 E_xx(1,i)/(E_xx(1,i)^2+E_xx(3,i)^2) 0];
s=R+H*P_p*H';
K=P_p*H'*inv(s);
E_xx(:,i)=E_xx(:,i)+K*([l(i);theta(i)]-h);
P=P_p-K*H*P_p;
mekf=mekf+((E_xx(1,i)-xx(1,i))^2+(E_xx(3,i)-xx(3,i))^2)/2;
end
E_e_x=xx(1,:)-E_xx(1,:);
E_e_y=xx(3,:)-E_xx(3,:);
e_x=xx(1,:)-z(1,:);
e_y=xx(3,:)-z(2,:);
toc;
mekf=sqrt(mekf/N);
mekf
%PF
pfxArr=zeros(4,N);
tic;
MC_number=5;
numSamples=500;
for m=1:MC_number
pfxArr(:,1)=[100 1 100 1]';
for i=1:numSamples
pfxpart(:,i)=[100 1 100 1]'+sqrt(P)*randn(4,1);
end
for k=2:N
for i = 1:numSamples
pfxpartupdate(:,i) = F * pfxpart(:,i)+W*(sqrt(Qc)*randn(2,1));
pfypart=[sqrt(pfxpartupdate(1,i)^2+pfxpartupdate(3,i)^2);atan2(pfxpartupdate(3,i),pfxpartupdate(1,i))];
pfdeta = [l(k);theta(k)] - pfypart;
pfq(i)=exp(-0.5*pfdeta'*inv(R)*pfdeta);
end
%权值归一化
pfqsum = sum(pfq);
pfq = pfq/pfqsum;
%残差重采样方法
pfx=zeros(4,1);
for i=1:numSamples
pfx=pfx+pfq(i)*pfxpartupdate(:,i);
end
pfxArr(:,k)= pfxArr(:,k)+pfx;
[pfq,II] = sort(pfq);
j = 1;i = numSamples;
while (i > 0)
mmm = ceil(pfq(i)*numSamples);
while (mmm > 0)
pfxpart(:,j) = pfxpartupdate(:,II(i));%+randn(1,ss)*noisebaseQ;
j = j+1;
mmm = mmm -1;
if j>numSamples
break;end
end
if j>numSamples break;end
i = i -1;
end
% 取粒子的平均值为粒子滤波的估计值
% pfx = (mean(pfxpart'))';
% pfxArr = [pfxArr pfx];
end
end
pfxArr(:,2:N)=pfxArr(:,2:N)/MC_number;
P_e_x=xx(1,:)-pfxArr(1,:);
P_e_y=xx(3,:)-pfxArr(3,:);
toc;
mpf=sqrt(sum(P_e_x.^2+P_e_y.^2)/2/N);
mpf
%UKF
tic;
X=[100 1 100 1]';
UX(:,1)=X;
nx=length(X);
beta=2;
%elfa=1*(10^-1);
elfa=0.001;
nx=size(X,1);
kappa=3-nx;
Q=[0.01^2 0 0 0;
0 0 0 0;
0 0 0.01^2 0;
0 0 0 0];
%lambda=((elfa^2)*(nx+kappa)-nx);%在ut_weights里边计算
for k=2:1:N
[wm,wc,c]=ut_weights(nx,elfa,beta,kappa);%计算权重
XX=ut_sigmas(X,P,c);%计算sigma点,XX是sigma点,X状态向量,P是方差
%size(XX)=4*9
[X,P,zz,Pzz,Pxz]=ukf_predict3(XX,wm,wc,Q,R,nx);
zrl=[l(k);theta(k)];
[X,P]=ukf_update1(Pxz,Pzz,P,zrl,zz,X);
UX(:,k)=X;
mukf=mukf+((UX(1,k)-xx(1,k))^2+(UX(3,k)-xx(3,k))^2)/2;
end
U_e_x=xx(1,:)-UX(1,:);
U_e_y=xx(3,:)-UX(3,:);
toc;
mukf=sqrt(mukf/N);
mukf
figure(1)
plot(xx(1,:),xx(3,:),'k-','LineWidth',2);
hold on
plot(E_xx(1,:),E_xx(3,:),'c--','LineWidth',2);
hold on
plot(pfxArr(1,:),pfxArr(3,:),'b:','LineWidth',2);
hold on
plot(UX(1,:),UX(3,:),'r-','LineWidth',2);
plot(z(1,:),z(2,:),'g+','LineWidth',2)
legend('真实值','EKF','PF','UKF','观测值');
xlabel('X/m');
ylabel('Y/m');
axis equal
figure(2)
tt=zeros(1,N);
subplot(2,1,1)
plot(E_e_x,'c--','LineWidth',2);hold on
plot(P_e_x,'b:','LineWidth',2);hold on
plot(U_e_x,'r-','LineWidth',2);hold on
plot(tt,'k-');
xlabel('T/s');
ylabel('d/m');
subplot(2,1,2)
plot(E_e_y,'c--','LineWidth',2);hold on
plot(P_e_y,'b:','LineWidth',2);hold on
plot(U_e_y,'r-','LineWidth',2);hold on
plot(tt,'k-');
xlabel('T/s');
ylabel('d/m');
legend('EKF','PF','UKF');
评论2