【滤波跟踪】基于粒子滤波实现机器人航迹跟踪附matlab代码
【滤波跟踪】基于粒子滤波实现机器人航迹跟踪附matlab代码
TT_Matlab
博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，完整matlab代码或者程序定制加qq1575304183。
✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
个人主页： Matlab科研工作室
个人信条：格物致知。
更多Matlab仿真内容点击
智能优化算法 神经网络预测 雷达通信 无线传感器 电力系统
信号处理 图像处理 路径规划 元胞自动机 无人机
⛄ 内容介绍
运动目标跟踪是计算机视觉研究领域中的一个核心课题,它在民用和军事等众多领域均具有广泛的应用价值。近年来,随着计算机技术的快速发展与现实生活和工作中的迫切需求,运动目标跟踪技术成为计算机视觉研究中的一个热点问题。
⛄ 部分代码
clear;
close all;
clc;
global dt;
time = 0;
endtime = 25; % [sec]
dt = 0.1; % [sec]
nSteps = ceil((endtime - time)/dt); %250步
result.time = [];
result.xTrue = [];
result.xEst = [];
result.mu = [];
% State Vector [x y yaw]’
xEst = [0 0 0]’;
% True State
xTrue = xEst;
% Covariance Matrix for predict
Q = diag([0.1 0.1 toRadian(3)]).^2; %这个平方是计算矩阵中每个元素的平方
% Covariance Matrix for observation
R = diag([1 toRadian(45)]).^2; % range[m], Angle[rad]
% Simulation parameter
global Qsigma
Qsigma = diag([0.1 toRadian(5)]).^2;
global Rsigma
Rsigma = diag([0.1 toRadian(1)]).^2;
%LM标签位置 [x, y] 也就是那三个五角星
LM = [-5 4;
4 -5;
5 6];
MAX_RANGE = 10; %最大观测距离
NP = 50; %粒子个数
IndexMax = 1; %最大粒子索引？？？
px = repmat(xEst, 1, NP);%创建一个元素均为xEst的1*NP矩阵 维数3*NP
pw = zeros(1, NP) + 1 / NP;%加权变量
% 相对于观测值的flag
flag = zeros(size(LM, 1), NP); %% 维数3*NP
% 各粒子所具有的地标位置
mu = zeros(2 * NP, size(LM, 1)); %%维数2NP*3
% 各粒子具有的地标协方差矩阵
Sigma = 100000 * ones(2 * NP, 2 * size(LM, 1)); %% 维数2NP*6
% Animation
AnimationFlag = true;
⛄ 运行结果
⛄ 参考文献
[1] 刘馨, 裴海龙, 李坚强. 基于粒子滤波器的移动机器人轨迹预测应用[J]. 2008.
[2] 赵广帅、耿振野、崔林飞. 基于改进的粒子滤波蒙特卡洛定位算法研究[J]. 长春理工大学学报：自然科学版, 2020, 43(5):8.
[3] 李海燕. 基于粒子滤波器和径向基神经网络的MIMO-OFDM信道跟踪[D]. 山东大学, 2008.
[4] 苏丽娅. 基于粒子滤波的运动目标跟踪算法研究[D]. 天津理工大学.