ukf实测信号的预测 matlab,无迹卡尔曼滤波UKF无线传感器网络定位跟踪matlab源码实现.pdf...

%本例对基于量测非线性模型 (正切)，进行了仿真；通过对比分析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 次

fortime=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