%抗野值UKF
clear all;clc; close all;
%% 产生原始数据
T =0.9; % 采样周期
data_tmp=load('新建文件夹\UWB_dd.txt');
data=data_tmp(:,1:4)';
n=length(data);
global V0k
v_x = 0.01; v_y = 0.01; % X、Y和Z方向的速度
x0 = -3; y0 = 0; % 初始位置
G = [ T 0;
1 0;
0 T;
0 1];
q=diag([0.1,0.1]);
Q = G*q*G';
R_Q = 0.01; % R方向观测误差标准差
R =diag( [R_Q^2, R_Q^2, R_Q^2, R_Q^2]); % 观测噪声协方差阵
P0 = diag( [0.1^2, 0.1^2, 0.1^2, 0.1^2]);
X0 = [x0, v_x, y0, v_y]'; % 状态向量初值
MCNum = 1; % Monte Carlo仿真次数 仿真次数为1
% Monte Carlo 仿真的开始
for i = 1:1:MCNum
%% 产生观测数据
Z_Tu = data; % 这里是观测值
%%%%%%%----- 滤波初始化 -----%%%%%%%
xEst_UKF = X0; % 初始时刻不匹配
Pk_UKF= P0;
alpha = 0.01; %sigma点在x均值附近的分布程度 [0.0001, 1] 一般取小正值
belta = 2; % x正态分布时,最优beta = 2
kalpha = -1; % k的取值是 3-l 等等
% EKF UKF 滤波方程
for k = 1 : n
my_tk=k;
zu = Z_Tu(:, k); % Z_T:n*total 每1列为1个时刻的坐标数据
[xEst_UKF, Pk_UKF,dZk] = Fukf(xEst_UKF, Pk_UKF, zu, Q, R, alpha,....
belta, kalpha, T,'ffun','hfun',my_tk);
Xk_UKF(:,k) = xEst_UKF; % 保存滤波数据
end
end
[a,map_d,true_d]=xinhao; % 这是直接从数据中测量得到的
figure
plot(Xk_UKF(1,:)+0*0.135, Xk_UKF(3,:)+0*0.1, 'r*', 'LineWidth', 0.1); hold on;
plot(true_d(:,1),true_d(:,2),'k-','LineWidth', 2);hold on;%square
legend( 'UKF滤波轨迹','真实轨迹');
xlabel('x/m');ylabel('y/m');
grid on; box on;
xlim([-3.5,4]);
ylim([-4, 5]);
grid on; box on;
hold on;
%% 误差曲线图
t=1:length(Xk_UKF);
t=t*0.28;
A=[Xk_UKF(1,:)' Xk_UKF(3,:)'];
B=map_d;
dis_tmpA=true_d-A; dis_A=sqrt(dis_tmpA(:,1).^2+dis_tmpA(:,2).^2);
% % % % % dis_tmpB=true_d-B; dis_B=sqrt(dis_tmpB(:,1).^2+dis_tmpB(:,2).^2);
figure;
plot(t,dis_A,'r-', 'LineWidth', 2);hold on;
% % % % % plot(t,dis_B,'b--', 'LineWidth', 2);
xlim([0,180]);
legend('UKF误差');
ylabel('y/m');xlabel('t/s');
xlim([0,180]);
ylim([0, 2]);
grid on; box on;
hold on;
UKF普通滤波.rar_UKF_UKF;matlab
版权申诉
172 浏览量
2022-07-14
20:01:18
上传
评论
收藏 3KB RAR 举报
JonSco
- 粉丝: 76
- 资源: 1万+
评论0