%本例对基于量测非线性模型 (正切),进行了仿真;通过对比分析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
如果觉得《ukf实测信号的预测 matlab 无迹卡尔曼滤波UKF无线传感器网络定位跟踪matlab源码实现.pdf...》对你有帮助,请点赞、收藏,并留下你的观点哦!