Matlab：粒子滤波定位仿真

最新推荐文章于 2025-07-09 20:16:35 发布

原创

最新推荐文章于 2025-07-09 20:16:35 发布 · 3.1k 阅读

22 ·

CC 4.0 BY-SA版权

function [] = particle_filter_localization()
%PARTICLE_FILTER_LOCALIZATION Summary of this function goes here
%   Detailed explanation goes here

% -------------------------------------------------------------------------
% TASK for particle filter localization
% for robotic class in 2018 of ZJU

% Preparartion: 
% 1. you need to know how to code and debug in matlab
% 2. understand the theory of Monte Carlo

% Then complete the code by YOURSELF!
% -------------------------------------------------------------------------

close all;
clear all;

disp('Particle Filter program start!!')

%% initialization
tic;
time = 0;
endTime = 60; % second
global dt;
dt = 0.1; % second
 
nSteps = ceil((endTime - time)/dt);
  
localizer.time = [];
localizer.xEst = [];
localizer.xGnd = [];
localizer.xOdom = [];
localizer.z = [];
localizer.PEst=[];
localizer.u=[];

% Estimated State [x y yaw]'
xEst=[0 0 0]';
% GroundTruth State
xGnd = xEst;
% Odometry-only = Dead Re