无迹卡尔曼滤波UKF无线传感器网络定位跟踪matlab源码实现

%本例对基于量测非线性模型（正切），进行了仿真；通过对比分析EKF，UKF和PF粒子滤波的性能。仿真结果可以看出粒子滤波器比

UKF优越，UKF比EKF性能优越。可作为学习滤波器的参考资料。

%存在问题：目前修正效果还不够完美，滤波值在预测值的基础上有所改善，使其接近真实值。

clear all;

close all;

clc;%Clear command window.

st = 100; % simulation length(time)

MC=50; %仿真次数

dl=zeros(MC,st+1);

de=zeros(MC,st+1);

dp=zeros(MC,st+1);

%仿真10次

for time=1:MC

dl(time,1)=0;

de(time,1)=0;

dp(time,1)=0;

Q = 0.5; % process noise covariance

R = [3^2 0;

0 0.1745^2 ];% measurement noise covariance

x0 = [0,5,0,7]'; % initial state

x = x0;

xA = [x(1)];%Array:Save the true X -position

yA = [x(3)];%Array:Save Y-Position

xobs = [x(1)]; %观测到的坐标

yobs = [x(3)];

ZA = [];

%初始化系统方程系数CV线性模型

F=[ 1.0 1.0 0.0 0.0;

0.0 1.0 0.0 0.0;

0.0 0.0 1.0 1.0;

0.0 0.0 0.0 1.0];

G=[0.5 0.0;

1.0 0.0;

0.0 0.5;

0.0 1.0];

%事先得到整体过程的实际状态值和观测值

for k = 1 : st

%two equation

x = F * x + G * normrnd(0,Q,2,1); %状态方程

if x(1)>0 && x(3)>=0

z(:,:,k) =[sqrt(x(1)^2+x(3)^2) atan(x(3)/x(1))]' + [normrnd(0,3^2,1,1) normrnd(0,0.1745^2,1,1)]'; %观测

方程

xobs = [xobs z(1,1,k)*cos(z(2,1,k))];

yobs = [yobs z(1,1,k)*sin(z(2,1,k))];

end

if x(1)<0 && x(3)>=0

z(:,:,k) =[sqrt(x(1)^2+x(3)^2) (atan(x(3)/x(1))+pi)]' + [normrnd(0,3^2,1,1) normrnd(0,0.1745^2,1,1)]';

%观测方程

xobs = [xobs z(1,1,k)*cos(z(2,1,k))];

yobs = [yobs z(1,1,k)*sin(z(2,1,k))];

end

if x(1)<0 && x(3)<=0

z(:,:,k) =[sqrt(x(1)^2+x(3)^2) (atan(x(3)/x(1))-pi)]' + [normrnd(0,3^2,1,1) normrnd(0,0.1745^2,1,1)]';

%观测方程

xobs = [xobs z(1,1,k)*cos(z(2,1,k))];

yobs = [yobs z(1,1,k)*sin(z(2,1,k))];

end

if x(1)>0 && x(3)<=0

z(:,:,k) =[sqrt(x(1)^2+x(3)^2) atan(x(3)/x(1))]' + [normrnd(0,3^2,1,1) normrnd(0,0.1745^2,1,1)]'; %观测

方程

xobs = [xobs z(1,1,k)*cos(z(2,1,k))];

yobs = [yobs z(1,1,k)*sin(z(2,1,k))];

end

xA = [xA x(1)];

yA = [yA x(3)];

dl(time,k+1)= sqrt((xobs(k+1)-x(1))^2+(yobs(k+1)-x(3))^2);

end

%k = 0:st;

%plot(xA,yA,'b*',0,0,'ro');

%xlabel('x'); ylabel('y');

%legend('Target Position','Observation Position');

% UKF

Pu = [2,0,0,0;

0,0.5,0,0;

0,0,3,0;

0,0,0,0.5].^2; %协方差矩阵初始化

xgeu = x0;

xgeAu = [xgeu(1)]; %后面代表的是滤波后的估计位置

ygeAu = [xgeu(3)];

xPru = [xgeu(1)]; %代表每一步目标的预测位置

yPru = [xgeu(3)];

alpha = 0.01; %0.5

beta = 2;

nnn = 4;

kappa = -1;

lamda = alpha^2*(nnn+kappa) - nnn;

%一般的方法：W0=v/(v+n),Wi=0.5/(v+n),i=1,...,2n;一般(v+n)==3 %这儿n=4,v=-1;W0=-1/3,Wi=1/6

wm = lamda/(nnn+lamda);

wc = wm+(1+beta-alpha^2);

for i = 1:2*nnn

t = 1/(2*(lamda+nnn));

wm =[ wm t];

wc =[ wc t];

end

for k = 1 : st

% UKF滤波器n=4,a=0.01,b=2,r=4*0.01^2-4 ,w0m= wxx = [xgeu];

Psqrtm=(chol((nnn+lamda)*Pu))';

xPts=[zeros(size(Pu,1),1) -Psqrtm Psqrtm];

wxx = xPts + repmat(wxx,1,2*nnn+1);

for j = 1:2*nnn+1

wxx(:,j) = F * wxx(:,j);