【目标跟踪】基于扩展卡尔曼滤波器实现多机器人跟踪定位附matlab代码

✅作者简介：热爱科研的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.

⛄ 完整代码

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料