✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
系统所处环境的复杂性使得现在科技对目标跟踪精度的要求越来越高,而且单传感器状态的估计已经无法满足系统感知外部环境的需要.在此,研究了基于扩展卡尔曼滤波的多传感器目标跟踪方法.仿真表明,扩展卡尔曼滤波对于非线性系统跟踪的效果更好.
⛄ 部分代码
% The ANEES can have outliers which could potentially distort the
% appearance of its curves. Keep outlierThreshold 1000 for
% simulationLength<=50. Adjust as necessary.
outlierThreshold=25;
%##########################################################################
% generate random colors for the graphs
for i=1:length(robots)
randomColor(i,:)=[rand,rand,0];
end
%##########################################################################
% Ground Truths and EKF
figure;
set(gca,'fontsize',15);
for i=1:length(robots)
plot(robots(i).groundTruth(:,1),robots(i).groundTruth(:,2),'-o','color',randomColor(i,:));
hold on;
plot(robots(i).groundTruth(1,1),robots(i).groundTruth(1,2),'o','MarkerSize',12,'color',randomColor(i,:));
hold on;
text(robots(i).groundTruth(1,1),robots(i).groundTruth(1,2),['R',num2str(i),' start'],'fontsize',12,'fontweight','bold');
hold on;
plot(robots(i).mu(:,1),robots(i).mu(:,2),'--x','color',randomColor(i,:));
hold on;
end
title(['实际(solid-o)和EKF估计 (broken-x) of ',num2str(length(robots)),' Robots']);
xlabel('x (m)');
ylabel('y (m)');
%##########################################################################
% Ground Truths and encoder only
figure;
set(gca,'fontsize',15);
for i=1:length(robots)
plot(robots(i).groundTruth(:,1),robots(i).groundTruth(:,2),'-o','color',randomColor(i,:));
hold on;
plot(robots(i).groundTruth(1,1),robots(i).groundTruth(1,2),'o','MarkerSize',12,'color',randomColor(i,:));
hold on;
text(robots(i).groundTruth(1,1),robots(i).groundTruth(1,2),['R',num2str(i),' start'],'fontsize',12,'fontweight','bold');
hold on;
plot(robots(i).encoderPose(:,1),robots(i).encoderPose(:,2),'--x','color',randomColor(i,:));
hold on;
end
title(['实际(solid-o)和编码器估计 (broken-x) of ',num2str(length(robots)),' Robots']);
xlabel('x (m)');
ylabel('y (m)');
%##########################################################################
% Average Normalized Estimation Error Squared
figure;
set(gca,'fontsize',15);
% draw the upper and lower anees bounds
[lower_bound,upper_bound] = anees_bounds(numRuns);
for i=1:length(robots)
% remove outliers in anees
indices = robots(i).anees>outlierThreshold;
robots(i).anees(indices) = [];
% make a copy of robots(i).distanceTraveled.
% resize the copy to match the dimensions of the robots(i).anees vector
distanceTraveledANEES=robots(i).distanceTraveled;
distanceTraveledANEES(indices)=[];
plot(distanceTraveledANEES,upper_bound.*ones(1,length(distanceTraveledANEES)),'--k');
hold on;
plot(distanceTraveledANEES,robots(i).anees,'-x','color',randomColor(i,:));
hold on;
plot(distanceTraveledANEES,lower_bound.*ones(1,length(distanceTraveledANEES)),'--k');
hold on;
text(distanceTraveledANEES(end),robots(i).anees(end),['R',num2str(i)],'fontsize',12,'fontweight','bold');
hold on;
end
title(['平均归一化估计误差 ',num2str(length(robots)),' Robots']);
xlabel('机器人移动距离 (m)');
ylabel('阿尼斯');
%##########################################################################
% absolute errors in x coordinates
figure;
set(gca,'fontsize',15);
for i=1:length(robots)
plot(robots(i).distanceTraveled,robots(i).actualError(:,1),'-x','color',randomColor(i,:));
hold on;
text(robots(i).distanceTraveled(end),robots(i).actualError(end,1),['R',num2str(i)],'fontsize',12,'fontweight','bold');
hold on;
end
title(['X轴的实际误差 ',num2str(length(robots)),' Robots']);
xlabel('机器人移动距离');
ylabel('误差(m)');
%##########################################################################
% absolute errors in y coordinates
figure;
set(gca,'fontsize',15);
for i=1:length(robots)
plot(robots(i).distanceTraveled,robots(i).actualError(:,2),'-x','color',randomColor(i,:));
hold on;
text(robots(i).distanceTraveled(end),robots(i).actualError(end,2),['R',num2str(i)],'fontsize',12,'fontweight','bold');
hold on;
end
title(['Y轴的实际误差',num2str(length(robots)),' Robots']);
xlabel('机器人移动距离(m)');
ylabel('误差 (m)');
%##########################################################################
end
⛄ 运行结果
⛄ 参考文献
[1]周凯宁, 周希元. 利用扩展卡尔曼滤波器进行机动目标跟踪[J]. 无线电工程动态, 1991(4):4.
[2]伍明, 孙继银. 基于扩展式卡尔曼滤波的移动机器人未知环境下动态目标跟踪[J]. 机器人, 2010, 32(3):10.
[3]潘丽娜. 基于扩展卡尔曼滤波的多传感器目标跟踪[J]. 舰船电子工程, 2010(12):71-72.
