???????????????? 欢迎来到本博客 ❤️❤️????????
????博主优势： ???????????? 博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️ 座右铭： 行百里者，半于九十。
???????????? 本文目录如下： ????????????
目录
KEWLS和KEWLS-KF是一种新的状态估计方法，它们采用低维线性卡尔曼滤波器将单个传感器的测量外推/预测到单个估计瞬间。这些方法主要用于WLS多点定位方法，可以提高定位的精度和鲁棒性。
KEWLS和KEWLS-KF方法的核心是卡尔曼滤波器，它可以通过对系统动态模型和测量模型进行有效的状态估计。通过将单个传感器的测量外推/预测到单个估计瞬间，KEWLS和KEWLS-KF可以有效地处理传感器测量中的噪声和不确定性，从而提高定位的精度和鲁棒性。
与传统的WLS多点定位方法相比，KEWLS和KEWLS-KF方法具有更好的性能，尤其是在复杂环境和噪声干扰下。这些方法可以应用于各种领域，如室内定位、车辆定位和机器人定位等，为实时定位和导航提供了新的可能性。因此，KEWLS和KEWLS-KF方法在状态估计领域具有重要的研究意义和实际应用前景。
摘要
本文代码运行了一个变量定位模拟，其中四个独立的传感器测量它们与动态目标的各自距离。然而，这些测量是按顺序接收的。这些测量之间的时间差是恒定的，并被称为测量间延迟（IML）。这自然会导致定位估计的误差。
模拟了可变位置服务，以确定IML的影响，并提出了用于顺序异步定位的方法（KEWLS/KKF）。
该模型作为批量模拟运行，其中每个传感器提供其嘈杂的测量估计，并采用各种位置技术来提供最终的估计。
关键变量
首先，该模型被构建为跟踪一个对象在直线或圆形路径上的运动。可以调整变量以改变轨迹、动力学、噪声水平、速度等。
d_t = IML（秒）
r_z = 单个传感器测量噪声标准差
q = KEWLS卡尔曼外推过程Q的标准差/预测D
qt = CV模型中过程Q加速度的标准差
ang = 0.2；%圆形路径的角度
其他模拟设置变量
bl = 传感器之间的基线距离（米）
Lim = 旅行限制的基线倍增器
xV = 目标x轴速度（米/秒）
yV = 目标y轴速度（米/秒）
move_noise = 目标移动噪声标准差（米）
s_pos = 目标起始位置
delay = 1；%将d_t延迟测量（如果不需要延迟，则设为0）
模型
该模型生成了与各种定位方法的准确性和运行时间相关的各种图表。
结构
可以选择一个变量通过一系列循环进行变化，在这些循环中，所选择的变量将在“Var”的下一个数组值中循环，可以使用lin/log或手动设置来改变“Var”：
Var = linspace(0.001,0.1,No_loops); %从第二个值开始
Var = logspace(-2,3,No_loops); %从第二个值开始
Var = [0.001,.002,.003,.004,.005,.006,.007,.008,.009,...
.01,.02,.03,.04,.05,.06,.07,.08,.09,...
.1,.2,.3,.4,.5,.6,.7,.8,.9,1];
一定要在这里更改数值！
r_z = Var(loop);
ChangingVar = 'r_z';
设置^^这将允许在最后进行自动标记
No_loops = 28；%变量循环次数（如果手动设置了Var的值，应该等于length(Var)）
通过每个变量，模拟将重复“No_it”次，以计算平均值。
No_it = 100；%迭代次数
鉴于tic/toc函数在10毫秒以下的不准确性，可以进一步重复“No_times”次，以给出各个定位方法的可用时间估计。
No_times = 1；%重复进行tic/toc函数的次数
建议将其保留为1，除非需要专门测量计算时间。
过程
模型最初运行并使用“DelayedLaterationFunc”或“DelayedLaterationFunc_circ”（如果ang~=0）收集完整的嘈杂测量数据。
然后，这些数据被分别输入到各种定位方法中，以提供它们各自的估计。
对于每个变量值的每次迭代，都会重复这一过程，利用平均值来确定RMSE、路径和一般跟踪性能。
KEWLS和KKF
这些是采用低维线性卡尔曼滤波器将单个传感器测量外推/预测到单个估计瞬间，用于WLS多点定位方法的新方法。
结果
在“Results”目录下已经给出了多种场景的结果。
主函数部分代码：
clear all
clc
%%run variable loop
%%/
%here we will change a variable and run it through the 'loop'
No_loops = 28; %number of variable loops
No_it = 100; %number of iterations
No_times = 1; %number of times a process is repeated for tic/toc func
%loop storage
LS_RMSE = zeros(1,No_loops); %classic least squares
UKF_RMSE = zeros(1,No_loops); %classic Unscented Kalman filter
KEWLS_RMSE = zeros(1,No_loops); %proposed = Sequential extrapolation / batch estimation
KKF_RMSE = zeros(1,No_loops); %proposed but with an additional KF
SUKF_RMSE = zeros(1,No_loops); %sequential Unscented Kalman filter
LS_mean = cell(1,No_loops); %classic least squares
UKF_mean = cell(1,No_loops); %classic Unscented Kalman filter
KEWLS_mean = cell(1,No_loops); %proposed = Sequential extrapolation / batch estimation
KKF_mean = cell(1,No_loops); %proposed but with an additional KF
SUKF_mean = cell(1,No_loops); %sequential Unscented Kalman filter
LS_t = zeros(1,No_loops);
UKF_t = zeros(1,No_loops);
KEWLS_t = zeros(1,No_loops);
KKF_t= zeros(1,No_loops);
SUKF_t = zeros(1,No_loops);
%%// VARIABLES
bl = 10; %baseline
Lim = 1; %basline multiplier for travel limits
xV = 1;
yV = 1;
move_noise = 0; %Movement noise StD
s_pos = [0 ; 0]; %target start Position
delay = 1; % LEAVE AS 1 to delay the measurements by d_t or not
R = RRLH2D_positions(bl); %generate pos of rx's
.....
%% storage in cells for each variable iteration
posX = cell(1,250);
posY = cell(1,250);
M_posX = cell(1,250);
M_posY = cell(1,250);
Measurments = cell(1,250); % noisy measurments
True_distances = cell(1,250); % actual distances
True_dis_atEstPos = cell(1,250); % actual distances at the estimation point
%% Least squares and two step least squares
LS_estX = cell(1,250);
LS_estY = cell(1,250);
LS_Errors = cell(1,250);
LS_time = zeros(1,100);
%% Classic Unscented Kalman filter
UKF_estX = cell(1,250);
UKF_estY = cell(1,250);
UKF_Errors = cell(1,250);
UKF_time = zeros(1,100);
%% KEWLS
KEWLS_estX = cell(1,250);
KEWLS_estY = cell(1,250);
KEWLS_Errors = cell(1,250);
KEWLS_time = zeros(1,100);
%% KEWLS-KF (KKF)
KKF_estX = cell(1,250);
KKF_estY = cell(1,250);
KKF_Errors = cell(1,250);
KKF_time = zeros(1,100);
%% SUKF solution
SUKF_estX = cell(1,250);
SUKF_estY = cell(1,250);
SUKF_Errors = cell(1,250);
SUKF_time = zeros(1,100);
%%/// RUN ITERATIONS //
%% ITERATIONS
%% generate a circular path
C_path = 0;
if ang ~= 0
%distance per simulation time interval = time interval to mainatin 1m/s
Radius = 1/ang;
No_steps = (2*pi*Radius)/0.0001; %circumfrence distance/simulation time
C_path = circle_move(Radius,bl,No_steps);
end
%run through iterations
for it = 1:No_it
%Run the simulation to collect measurment data
if ang == 0
[True_est_pos, measurment_pos, measurments, true_distances, true_dis_atEstPos] =...
DelayedLaterationFunc_Str(R, delay, bl, Lim, xV, yV, d_t, s_pos, r_z, move_noise);
else
[True_est_pos, measurment_pos, measurments, true_distances, true_dis_atEstPos] =...
DelayedLaterationFunc_circ(R, delay, bl, Lim, xV, yV, d_t, s_pos, C_path, r_z, move_noise);
end
% store data
%legendCell{it} = append(num2str(d_t),'s'); %convert d_t to string for legend
posX{it} = True_est_pos(1,:);
posY{it} = True_est_pos(2,:);
M_posX{it} = measurment_pos(1,:);
M_posY{it} = measurment_pos(2,:);
Measurments{it} = measurments;
True_distances{it} = true_distances;
True_dis_atEstPos{it} = true_dis_atEstPos;
%% perform standard LS on the measurments - (currently TS is turned off for fair timing)
可视化部分代码：
%% RMSE error %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1)
hold on
plot(Var,LS_RMSE,'-.')
plot(Var,UKF_RMSE,'-.')
plot(Var,SUKF_RMSE, '--')
plot(Var,KEWLS_RMSE)
plot(Var,KKF_RMSE)
legend('LS','UKF','SUKF','KEWLS', 'K-KF')
ylabel('RMSE/m')
xlabel(label)
T = append('System RMSE with varied ', ChangingVar);
title(T)
hold off
%% execution time %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(2)
hold on
plot(Var,LS_t,'-.')
plot(Var,UKF_t,'-.')
plot(Var,SUKF_t, '--')
plot(Var,KEWLS_t)
plot(Var,KKF_t)
legend('LS','UKF','SUKF','KEWLS', 'K-KF')
ylabel('Time taken to compute all points/s')
xlabel(label)
T = append('Execution Time with varied ', ChangingVar);
title(T)
hold off
%% Last Loop avererage errors %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(3)
hold on
plot(LS_me,'-.')
plot(UKF_me,'-.')
plot(SUKF_me,'--')
plot(KEWLS_me)
plot(KKF_me)
legend('LS','UKF','SUKF','KEWLS','K-KF')
xlabel('Iteration')
T = append('Average Error with varied ', ChangingVar);
title(T)
hold off
%% Last Loop, last iteration, errors %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(4)
hold on
for i = 1:1%it
plot(LS_Errors{i}(1,3:end),'-.')
plot(UKF_Errors{i}(1,3:end),'-.')
plot(SUKF_Errors{i}(1,3:end),'--')
plot(KEWLS_Errors{i})
plot(KKF_Errors{i})
end
legend('LS','UKF','SUKF','KEWLS','K-KF')
ylabel('Error /m')
xlabel('Iteration')
T = append('Last iteration error with varied ', ChangingVar);
title(T)
hold off
%% Path Plot & Estimate points %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(5)
hold on
%plot user positions
for i = 1:1%iteration-1
plot(posX{i},posY{i},'+')
end
%plot LS estimates
for i = 1:1%iteration-1
plot(LS_estX{i},LS_estY{i},'*')
end
%plot proposed estimates
for i = 1:1%iteration-1
plot(KEWLS_estX{i},KEWLS_estY{i}, 'p')
end
%plot UKF estimates
for i = 1:1%iteration-1
plot(UKF_estX{i},UKF_estY{i}, 'x')
end
%plot UKF estimates
for i = 1:1%iteration-1
plot(SUKF_estX{i},SUKF_estY{i}, 'o')
end
%plots lines between each estimate and position.
for i = 1:1%iteration-1
for j = 1:length(LS_estX{i})
x = [LS_estX{i}(j) , posX{i}(j)];
y = [LS_estY{i}(j) , posY{i}(j)];
plot(x,y,'--','LineWidth',2)
end
end
%plots lines between each estimate and position.
for i = 1:1%iteration-1
for j = 1:length(KEWLS_estX{i}(1,1:end-2))
x = [KEWLS_estX{i}(j) , posX{i}(j+2)];
y = [KEWLS_estY{i}(j) , posY{i}(j+2)];
plot(x,y,'LineWidth',2)
end
end
%plots lines between each estimate and position.
for i = 1:1%iteration-1
for j = 1:length(SUKF_estX{i})
x = [SUKF_estX{i}(j) , posX{i}(j)];
y = [SUKF_estY{i}(j) , posY{i}(j)];
plot(x,y,'-.','LineWidth',2)
end
end
%plots lines between each estimate and position.
for i = 1:1%iteration-1
for j = 1:length(UKF_estX{i})
x = [UKF_estX{i}(j) , posX{i}(j)];
y = [UKF_estY{i}(j) , posY{i}(j)];
plot(x,y,'-.','LineWidth',2)
end
end
axis equal
scatter(R(1,:),R(2,:),100,'^','LineWidth',2) %show the RRLHs
%legend(legendCell)
legend('UT position','LS est', 'KEWLS', 'UKF' , 'SUKF')
T = append('Path plot with varied ', ChangingVar);
title(T)
xlabel('X position /m')
ylabel('Y position /m)')
hold off
%% Path Plot
figure(6)
hold on
plot(posX{1},posY{1})
plot(LS_estX{1},LS_estY{1})
plot(UKF_estX{1},UKF_estY{1})
plot(SUKF_estX{1},SUKF_estY{1})
plot(KEWLS_estX{1},KEWLS_estY{1})
plot(KKF_estX{1},KKF_estY{1})
axis equal
scatter(R(1,:),R(2,:),100,'^','LineWidth',2) %show the RRLHs
legend('Target Position','LS','UKF','SUKF','KEWLS','K-KF')
T = append('Path plot with varied ', ChangingVar);
title(T)
xlabel('X position /m')
ylabel('Y position /m)')
hold off
figure(10)
loglog(Var,LS_RMSE,'-.')
hold on
loglog(Var,UKF_RMSE,'-.')
loglog(Var,SUKF_RMSE, '--')
loglog(Var,KEWLS_RMSE)
loglog(Var,KKF_RMSE)
legend('LS','UKF','SUKF','KEWLS','K-KF')
ylabel('RMSE/m')
xlabel(label)
T = append('System RMSE with varied ', ChangingVar);
title(T)
hold off
文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。
[1]陈鹏,钱徽,朱淼良.基于加权最小二乘的卡尔曼滤波算法[J].计算机科学, 2009.DOI:JournalArticle/5af37a83c095d718d80ccec5.
[2]张肖雄.基于卡尔曼滤波的系统状态和荷载识别方法研究[D].湖南大学,2019.
[3]陈鹏,钱徽,朱淼良.基于加权最小二乘的卡尔曼滤波算法[J].计算机科学, 2009(11):236-237+263.DOI:CNKI:SUN:JSJA.0.2009-11-059.
