setParticles
Description
Examples
Set Particles from Monte Carlo Localization Algorithm
Set particles from the particle filter used in the monteCarloLocalization
object.
Create a map and a monteCarloLocalization
object.
sm = likelihoodFieldSensorModel; sm.Map = binaryOccupancyMap(10,10,20); mcl = monteCarloLocalization(UseLidarScan=true); mcl.SensorModel = sm;
Call the reset method to initialize the mcl
. The monteCarloLocalization
object must be initialized using the step
or reset
methods first before you can use the setParticles
function.
reset(mcl);
Load particle states and weights to use to set the particles in the monteCarloLocalization
object.
load("particleInfo.mat")
setParticles(mcl,particleStates,weights);
Create lidar scan data. Create 300 range with measured values of 10 meters. Set the middle values to 2 meters to represent an obstacle. Create corresponding angles for the range readings and set the angle range to be -pi/2
to pi/2
radians.
ranges = 10*ones(1,300); ranges(1,130:170) = 2.0; angles = linspace(-pi/2,pi/2,300); scan = lidarScan(ranges,angles);
Call mcl
on a pose at the origin with the created lidar scan data.
[isUpdated,particlePoses,covariance] = mcl([0 0 0],scan)
isUpdated = logical
1
particlePoses = 1×3
0.0075 0.0518 -0.0010
covariance = 3×3
0.8914 0.0100 0
0.0100 0.9305 0
0 0 0.9823
Input Arguments
mcl
— monteCarloLocalization
object
handle
monteCarloLocalization
object, specified as an
object handle.
particles
— Estimation particles
n-by-3 matrix
Estimation particles, specified as an n-by-3 matrix, where each
row corresponds to the position and orientation of a single particle [x y
theta]
. n is the total number of particles to set and
must be equal to the number of particles in mcl
. Use the output
particle poses and weights from getParticles
to determine the number of particles at the current timestep
and set particle poses and weights.
weights
— Weights of particles
n-element column vector
Weights of particles, returned as a n-element column vector. Each
row corresponds to the weight of the particle in the matching row of
particles
. These weights are used in the final estimate of the
pose of the vehicle. n is the total number of weights to set and must
be equal to the number of particles in mcl
. Use the output particle
poses and weights from getParticles
to determine the number of
particles at the current timestep and set particle poses and weights.
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Version History
Introduced in R2024a
MATLAB 명령
다음 MATLAB 명령에 해당하는 링크를 클릭했습니다.
명령을 실행하려면 MATLAB 명령 창에 입력하십시오. 웹 브라우저는 MATLAB 명령을 지원하지 않습니다.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)