Assignment code for Georgia Tech CS 3630, 2014
Fixed translation and scaling issue with particles
+17
-686
-636
Assignment_3/code/matlab/Differential.asv
-636
Assignment_3/code/matlab/Differential.asv
···
1
-
%Vehicle differential drive vehicle class
2
-
%
3
-
% This class models the kinematics of a differential vehicle (unicycle model). For
4
-
% given omegaing and velocity inputs it updates the true vehicle state and returns
5
-
% noise-corrupted odometry readings.
6
-
%
7
-
% Methods::
8
-
% init initialize vehicle state
9
-
% f predict next state based on odometry
10
-
% step move one time step and return noisy odometry
11
-
% control generate the control inputs for the vehicle
12
-
% update update the vehicle state
13
-
% run run for multiple time steps
14
-
% Fx Jacobian of f wrt x
15
-
% Fv Jacobian of f wrt odometry noise
16
-
% gstep like step() but displays vehicle
17
-
% plot plot/animate vehicle on current figure
18
-
% plot_xy plot the true path of the vehicle
19
-
% add_driver attach a driver object to this vehicle
20
-
% display display state/parameters in human readable form
21
-
% char convert to string
22
-
%
23
-
% Static methods::
24
-
% plotv plot/animate a pose on current figure
25
-
%
26
-
% Properties (read/write)::
27
-
% x true vehicle state (3x1)
28
-
% V odometry covariance (2x2)
29
-
% odometry distance moved in the last interval (2x1)
30
-
% rdim dimension of the robot (for drawing)
31
-
% L length of the vehicle (wheelbase)
32
-
% alphalim omegaing wheel limit
33
-
% maxspeed maximum vehicle speed
34
-
% T sample interval
35
-
% verbose verbosity
36
-
% x_hist history of true vehicle state (Nx3)
37
-
% driver reference to the driver object
38
-
% x0 initial state, restored on init()
39
-
%
40
-
% Examples::
41
-
%
42
-
% Create a vehicle with odometry covariance
43
-
% v = Differential( diag([0.1 0.01].^2 );
44
-
% and display its initial state
45
-
% v
46
-
% now apply a speed (0.2m/s) and omega angle (0.1rad) for 1 time step
47
-
% odo = v.update([0.2, 0.1])
48
-
% where odo is the noisy odometry estimate, and the new true vehicle state
49
-
% v
50
-
%
51
-
% We can add a driver object
52
-
% v.add_driver( RandomPath(10) )
53
-
% which will move the vehicle within the region -10<x<10, -10<y<10 which we
54
-
% can see by
55
-
% v.run(1000)
56
-
% which shows an animation of the vehicle moving between randomly
57
-
% selected wayoints.
58
-
%
59
-
% Notes::
60
-
% - Subclasses the MATLAB handle class which means that pass by reference semantics
61
-
% apply.
62
-
%
63
-
% Reference::
64
-
%
65
-
% Robotics, Vision & Control,
66
-
% Peter Corke,
67
-
% Springer 2011
68
-
%
69
-
% See also RandomPath, EKF.
70
-
71
-
% Copyright (C) 1993-2011, by Peter I. Corke
72
-
%
73
-
% This file is part of The Robotics Toolbox for Matlab (RTB).
74
-
%
75
-
% RTB is free software: you can redistribute it and/or modify
76
-
% it under the terms of the GNU Lesser General Public License as published by
77
-
% the Free Software Foundation, either version 3 of the License, or
78
-
% (at your option) any later version.
79
-
%
80
-
% RTB is distributed in the hope that it will be useful,
81
-
% but WITHOUT ANY WARRANTY; without even the implied warranty of
82
-
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
83
-
% GNU Lesser General Public License for more details.
84
-
%
85
-
% You should have received a copy of the GNU Leser General Public License
86
-
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
87
-
88
-
classdef Differential < handle
89
-
90
-
properties
91
-
% state
92
-
x % true state (x,y,theta)
93
-
x_hist % x history
94
-
95
-
% parameters
96
-
L % length of vehicle
97
-
alphalim % omegaing wheel limit
98
-
maxspeed % maximum speed
99
-
dim % dimension of the world -dim -> +dim in x and y
100
-
rdim % dimension of the robot
101
-
dt % sample interval
102
-
V % odometry covariance
103
-
104
-
odometry % distance moved in last interval
105
-
106
-
verbose
107
-
108
-
driver % driver object
109
-
x0 % initial state
110
-
end
111
-
112
-
methods
113
-
114
-
function veh = Differential(V, varargin)
115
-
%Differential Vehicle object constructor
116
-
%
117
-
% V = Differential(V_ACT, OPTIONS) creates a Differential object with actual odometry
118
-
% covariance V_ACT (2x2) matrix corresponding to the odometry vector [dx dtheta].
119
-
%
120
-
% Options::
121
-
% 'stlim',A omegaing angle limit (default 0.5 rad)
122
-
% 'vmax',S Maximum speed (default 5m/s)
123
-
% 'L',L Wheel base (default 1m)
124
-
% 'x0',x0 Initial state (default (0,0,0) )
125
-
% 'dt',T Time interval
126
-
% 'rdim',R Robot size as fraction of plot window (default 0.2)
127
-
% 'verbose' Be verbose
128
-
%
129
-
% Notes::
130
-
% - Subclasses the MATLAB handle class which means that pass by reference semantics
131
-
% apply.
132
-
133
-
if ~isnumeric(V)
134
-
error('first arg is V');
135
-
end
136
-
veh.x = zeros(3,1);
137
-
if nargin < 1
138
-
V = zeros(2,2);
139
-
end
140
-
141
-
opt.stlim = 0.5;
142
-
opt.vmax = 5;
143
-
opt.L = 1;
144
-
opt.rdim = 0.2;
145
-
opt.dt = 0.1;
146
-
opt.x0 = zeros(3,1);
147
-
opt = tb_optparse(opt, varargin);
148
-
149
-
veh.V = V;
150
-
151
-
veh.dt = opt.dt;
152
-
veh.alphalim = opt.stlim;
153
-
veh.maxspeed = opt.vmax;
154
-
veh.L = opt.L;
155
-
veh.x0 = opt.x0(:);
156
-
veh.rdim = opt.rdim;
157
-
veh.verbose = opt.verbose;
158
-
159
-
veh.x_hist = [];
160
-
end
161
-
162
-
function init(veh, x0)
163
-
%Differential.init Reset state of vehicle object
164
-
%
165
-
% V.init() sets the state V.x := V.x0, initializes the driver
166
-
% object (if attached) and clears the history.
167
-
%
168
-
% V.init(X0) as above but the state is initialized to X0.
169
-
if nargin > 1
170
-
veh.x = x0(:);
171
-
else
172
-
veh.x = veh.x0;
173
-
end
174
-
veh.x_hist = [];
175
-
if ~isempty(veh.driver)
176
-
veh.driver.init()
177
-
end
178
-
end
179
-
180
-
function add_driver(veh, driver)
181
-
%Differential.add_driver Add a driver for the vehicle
182
-
%
183
-
% V.add_driver(D) connects a driver object D to the vehicle. The driver
184
-
% object has one public method:
185
-
% [speed, omega] = D.demand();
186
-
% that returns a speed and omega angle.
187
-
%
188
-
% Notes::
189
-
% - The Differential.step() method invokes the driver if one is attached.
190
-
%
191
-
% See also Differential.step, RandomPath.
192
-
veh.driver = driver;
193
-
driver.veh = veh;
194
-
end
195
-
196
-
function xnext = f(veh, x, odo, w)
197
-
%Differential.f Predict next state based on odometry
198
-
%
199
-
% XN = V.f(X, ODO) predict next state XN (1x3) based on current state X (1x3) and
200
-
% odometry ODO (1x2) is [distance,change_heading].
201
-
%
202
-
% XN = V.f(X, ODO, W) as above but with odometry noise W.
203
-
%
204
-
% Notes::
205
-
% - Supports vectorized operation where X and XN (Nx3).
206
-
if nargin < 4
207
-
w = [0 0];
208
-
end
209
-
210
-
dd = odo(1) + w(1); dth = odo(2);
211
-
thp = x(:,3) + dth;
212
-
xnext = x + [(dd+w(1))*cos(thp) (dd+w(1))*sin(thp) ones(size(x,1),1)*dth+w(2)];
213
-
end
214
-
215
-
function odo = update(veh, u)
216
-
%Differential.update Update the vehicle state
217
-
%
218
-
% ODO = V.update(U) is the true odometry value for
219
-
% motion with U=[speed,omega].
220
-
%
221
-
% Notes::
222
-
% - Appends new state to state history property x_hist.
223
-
% - Odometry is also saved as property odometry.
224
-
225
-
vxScale = 0.004;
226
-
omegaScale = 0.0475;
227
-
228
-
% get motor values (Left-Right wrpt Fluke)
229
-
uL=u(1);
230
-
uR=u(2);
231
-
232
-
% Measure constants
233
-
r = 1.375; %% Wheel Radius: 1.375 inches
234
-
L = 5.75; %% Wheel Base Wdith: 5.75 inches
235
-
236
-
% calculate speed
237
-
vx = 0.004 * (r / 2) * (uR + uL); %% velocity
238
-
omega = 0.0475 * (r / L) * (uR - uL); %% twist
239
-
240
-
% update new state using Euler integration, not expmap
241
-
xp = veh.x; % previous state
242
-
if omega ~= 0
243
-
R = (vx / omega);
244
-
else
245
-
R = vx;
246
-
end
247
-
dx = [vx; 0] + R * [sin(omega * veh.dt);
248
-
1 - cos(omega * veh.dt)];
249
-
250
-
pause(0.25);
251
-
252
-
theta = xp(3);
253
-
rot = [cos(theta), -sin(theta);
254
-
sin(theta), cos(theta)];
255
-
256
-
dxw = rot * dx;
257
-
258
-
veh.x(1) = xp(1) + dxw(1); %% new x position
259
-
veh.x(2) = xp(2) + dxw(2); %% new y position
260
-
veh.x(3) = xp(3) + omega * veh.dt; %% new theta rotation
261
-
262
-
odo = [vx*veh.dt omega*veh.dt];
263
-
veh.odometry = odo;
264
-
265
-
veh.x_hist = [veh.x_hist; veh.x']; % maintain history
266
-
end
267
-
268
-
269
-
function J = Fx(veh, x, odo)
270
-
%Differential.Fx Jacobian df/dx
271
-
%
272
-
% J = V.Fx(X, ODO) is the Jacobian df/dx (3x3) at the state X, for
273
-
% odometry input ODO.
274
-
%
275
-
% See also Differential.f, Differential.Fv.
276
-
dd = odo(1); dth = odo(2);
277
-
thp = x(3) + dth;
278
-
279
-
J = [
280
-
1 0 -dd*sin(thp)
281
-
0 1 dd*cos(thp)
282
-
0 0 1
283
-
];
284
-
end
285
-
286
-
function J = Fv(veh, x, odo)
287
-
%Differential.Fv Jacobian df/dv
288
-
%
289
-
% J = V.Fv(X, ODO) returns the Jacobian df/dv (3x2) at the state X, for
290
-
% odometry input ODO.
291
-
%
292
-
% See also Differential.F, Differential.Fx.
293
-
dd = odo(1); dth = odo(2);
294
-
thp = x(3) + dth;
295
-
296
-
J = [
297
-
cos(thp) -dd*sin(thp)
298
-
sin(thp) dd*cos(thp)
299
-
0 1
300
-
];
301
-
end
302
-
303
-
function odo = step(veh, varargin)
304
-
%Differential.step Advance one timestep
305
-
%
306
-
% ODO = V.step(SPEED, omega) updates the vehicle state for one timestep
307
-
% of motion at specified SPEED and omega angle, and returns noisy odometry.
308
-
%
309
-
% ODO = V.step() updates the vehicle state for one timestep of motion and
310
-
% returns noisy odometry. If a "driver" is attached then its DEMAND() method
311
-
% is invoked to compute speed and omega angle. If no driver is attached
312
-
% then speed and omega angle are assumed to be zero.
313
-
%
314
-
% Notes::
315
-
% - Noise covariance is the property V.
316
-
%
317
-
% See also Differential.control, Differential.update, Differential.add_driver.
318
-
319
-
% get the control input to the vehicle from either passed demand or driver
320
-
u = veh.control(varargin{:});
321
-
322
-
% compute the true odometry and update the state
323
-
odo = veh.update(u);
324
-
325
-
% add noise to the odometry
326
-
if veh.V
327
-
odo = veh.odometry + randn(1,2)*veh.V;
328
-
end
329
-
end
330
-
331
-
332
-
function u = control(veh, speed, omega)
333
-
%Differential.control Compute the control input to vehicle
334
-
%
335
-
% U = V.control(SPEED, omega) returns a control input (speed,omega)
336
-
% based on provided controls SPEED,omega to which speed and omegaing
337
-
% angle limits have been applied.
338
-
%
339
-
% U = V.control() returns a control input (speed,omega) from a "driver"
340
-
% if one is attached, the driver's DEMAND() method is invoked. If no driver is attached
341
-
% then speed and omega angle are assumed to be zero.
342
-
%
343
-
% See also Differential.step, RandomPath.
344
-
if nargin < 2
345
-
% if no explicit demand, and a driver is attached, use
346
-
% it to provide demand
347
-
if ~isempty(veh.driver)
348
-
u = veh.driver.demand();
349
-
else
350
-
% no demand, do something safe
351
-
u = [0;0];
352
-
end
353
-
end
354
-
355
-
% clip both controls
356
-
u(1) = min(200, max(-200, u(1)));
357
-
u(2) = min(200, max(-200, u(2)));
358
-
end
359
-
360
-
function p = run(veh, nsteps)
361
-
%Differential.run Run the vehicle simulation
362
-
%
363
-
% V.run(N) runs the vehicle model for N timesteps and plots
364
-
% the vehicle pose at each step.
365
-
%
366
-
% P = V.run(N) runs the vehicle simulation for N timesteps and
367
-
% return the state history (Nx3) without plotting. Each row
368
-
% is (x,y,theta).
369
-
%
370
-
% See also Differential.step.
371
-
372
-
if nargin < 2
373
-
nsteps = 1000;
374
-
end
375
-
376
-
%veh.clear();
377
-
if ~isempty(veh.driver)
378
-
veh.driver.visualize();
379
-
end
380
-
381
-
veh.visualize();
382
-
for i=1:nsteps
383
-
veh.step();
384
-
if nargout == 0
385
-
% if no output arguments then plot each step
386
-
veh.plot();
387
-
drawnow
388
-
end
389
-
end
390
-
p = veh.x_hist;
391
-
end
392
-
393
-
% TODO run and run2 should become superclass methods...
394
-
395
-
function p = run2(veh, T, x0, speed, omega)
396
-
%Differential.run2 Run the vehicle simulation
397
-
%
398
-
% P = V.run2(T, X0, SPEED, omega) runs the vehicle model for a time T with
399
-
% speed SPEED and omegaing angle omega. P (Nx3) is the path followed and
400
-
% each row is (x,y,theta).
401
-
%
402
-
% Notes::
403
-
% - Faster and more specific version of run() method.
404
-
%
405
-
% See also Differential.run, Differential.step.
406
-
veh.init(x0);
407
-
408
-
for i=1:(T/veh.dt)
409
-
veh.update([speed omega]);
410
-
end
411
-
p = veh.x_hist;
412
-
end
413
-
414
-
function h = plot(veh, varargin)
415
-
%Differential.plot Plot vehicle
416
-
%
417
-
% V.plot(OPTIONS) plots the vehicle on the current axes at a pose given by
418
-
% the current state. If the vehicle has been previously plotted its
419
-
% pose is updated. The vehicle is depicted as a narrow triangle that
420
-
% travels "point first" and has a length V.rdim.
421
-
%
422
-
% V.plot(X, OPTIONS) plots the vehicle on the current axes at the pose X.
423
-
424
-
% H = V.plotv(X, OPTIONS) draws a representation of a ground robot as an
425
-
% oriented triangle with pose X (1x3) [x,y,theta]. H is a graphics handle.
426
-
%
427
-
% V.plotv(H, X) as above but updates the pose of the graphic represented
428
-
% by the handle H to pose X.
429
-
%
430
-
% Options::
431
-
% 'scale',S Draw vehicle with length S x maximum axis dimension
432
-
% 'size',S Draw vehicle with length S
433
-
% 'color',C Color of vehicle.
434
-
% 'fill' Filled
435
-
%
436
-
% See also Differential.plotv.
437
-
438
-
h = findobj(gcf, 'Tag', 'Differential.plot');
439
-
if isempty(h)
440
-
% no instance of vehicle graphical object found
441
-
h = Differential.plotv(veh.x, varargin{:});
442
-
set(h, 'Tag', 'Differential.plot'); % tag it
443
-
end
444
-
445
-
if ~isempty(varargin) && isnumeric(varargin{1})
446
-
% V.plot(X)
447
-
Differential.plotv(h, varargin{1}); % use passed value
448
-
else
449
-
% V.plot()
450
-
Differential.plotv(h, veh.x); % use current state
451
-
end
452
-
end
453
-
454
-
function out = plot_xy(veh, varargin)
455
-
%Differential.plot_xy Plots true path followed by vehicle
456
-
%
457
-
% V.plot_xy() plots the true xy-plane path followed by the vehicle.
458
-
%
459
-
% V.plot_xy(LS) as above but the line style arguments LS are passed
460
-
% to plot.
461
-
%
462
-
% Notes::
463
-
% - The path is extracted from the x_hist property.
464
-
465
-
xyt = veh.x_hist;
466
-
if nargout == 0
467
-
plot(xyt(:,1), xyt(:,2), varargin{:});
468
-
else
469
-
out = xyt;
470
-
end
471
-
end
472
-
473
-
function visualize(veh)
474
-
grid on
475
-
end
476
-
477
-
function verbosity(veh, v)
478
-
%Differential.verbosity Set verbosity
479
-
%
480
-
% V.verbosity(A) set verbosity to A. A=0 means silent.
481
-
veh.verbose = v;
482
-
end
483
-
484
-
function display(nav)
485
-
%Differential.display Display vehicle parameters and state
486
-
%
487
-
% V.display() displays vehicle parameters and state in compact
488
-
% human readable form.
489
-
%
490
-
% Notes::
491
-
% - This method is invoked implicitly at the command line when the result
492
-
% of an expression is a Differential object and the command has no trailing
493
-
% semicolon.
494
-
%
495
-
% See also Differential.char.
496
-
497
-
loose = strcmp( get(0, 'FormatSpacing'), 'loose');
498
-
if loose
499
-
disp(' ');
500
-
end
501
-
disp([inputname(1), ' = '])
502
-
disp( char(nav) );
503
-
end % display()
504
-
505
-
function s = char(veh)
506
-
%Differential.char Convert to a string
507
-
%
508
-
% s = V.char() is a string showing vehicle parameters and state in in
509
-
% a compact human readable format.
510
-
%
511
-
% See also Differential.display.
512
-
513
-
s = 'Differential object';
514
-
s = char(s, sprintf(...
515
-
' L=%g, maxspeed=%g, alphalim=%g, T=%f, nhist=%d', ...
516
-
veh.L, veh.maxspeed, veh.alphalim, veh.dt, ...
517
-
numrows(veh.x_hist)));
518
-
if ~isempty(veh.V)
519
-
s = char(s, sprintf(...
520
-
' V=(%g,%g)', ...
521
-
veh.V(1,1), veh.V(2,2)));
522
-
end
523
-
s = char(s, sprintf(' x=%g, y=%g, theta=%g', veh.x));
524
-
if ~isempty(veh.driver)
525
-
s = char(s, ' driven by::');
526
-
s = char(s, [[' '; ' '] char(veh.driver)]);
527
-
end
528
-
end
529
-
530
-
end % method
531
-
532
-
methods(Static)
533
-
534
-
function h_ = plotv(x, varargin)
535
-
%Differential.plotv Plot ground vehicle pose
536
-
%
537
-
% H = Differential.plotv(X, OPTIONS) draws a representation of a ground robot as an
538
-
% oriented triangle with pose X (1x3) [x,y,theta]. H is a graphics handle.
539
-
% If X (Nx3) is a matrix it is considered to represent a trajectory in which case
540
-
% the vehicle graphic is animated.
541
-
%
542
-
% Differential.plotv(H, X) as above but updates the pose of the graphic represented
543
-
% by the handle H to pose X.
544
-
%
545
-
% Options::
546
-
% 'scale',S Draw vehicle with length S x maximum axis dimension
547
-
% 'size',S Draw vehicle with length S
548
-
% 'color',C Color of vehicle.
549
-
% 'fill' Filled with solid color as per 'color' option
550
-
% 'fps',F Frames per second in animation mode (default 10)
551
-
%
552
-
% Example::
553
-
%
554
-
% Generate some path 3xN
555
-
% p = PRM.plan(start, goal);
556
-
% Set the axis dimensions to stop them rescaling for every point on the path
557
-
% axis([-5 5 -5 5]);
558
-
%
559
-
% Now invoke the static method
560
-
% Differential.plotv(p);
561
-
%
562
-
% Notes::
563
-
% - This is a static method.
564
-
565
-
if isscalar(x) && ishandle(x)
566
-
% plotv(h, x)
567
-
h = x;
568
-
x = varargin{1};
569
-
x = x(:)';
570
-
T = transl([x(1:2) 0]) * trotz( x(3) );
571
-
set(h, 'Matrix', T);
572
-
return
573
-
end
574
-
575
-
opt.scale = 1/60;
576
-
opt.size = [];
577
-
opt.fill = false;
578
-
opt.color = 'r';
579
-
opt.fps = 10;
580
-
581
-
[opt,args] = tb_optparse(opt, varargin);
582
-
583
-
lineprops = { 'Color', opt.color' };
584
-
if opt.fill
585
-
lineprops = [lineprops 'fill' opt.color ];
586
-
end
587
-
588
-
589
-
% compute the dimensions of the robot
590
-
if ~isempty(opt.size)
591
-
d = opt.size;
592
-
else
593
-
% get the current axes dimensions
594
-
a = axis;
595
-
d = (a(2)+a(4) - a(1)-a(3)) * opt.scale;
596
-
end
597
-
598
-
% draw it
599
-
points = [
600
-
d 0
601
-
-d -0.6*d
602
-
-d 0.6*d
603
-
]';
604
-
605
-
h = hgtransform();
606
-
hp = plot_poly(points, lineprops{:});
607
-
for hh=hp
608
-
set(hh, 'Parent', h);
609
-
end
610
-
611
-
if (numel(x) > 3) && (numcols(x) == 3)
612
-
% animation mode
613
-
for i=1:numrows(x)
614
-
T = transl([x(i,1:2) 0]) * trotz( x(i,3) );
615
-
set(h, 'Matrix', T);
616
-
pause(1/opt.fps);
617
-
end
618
-
elseif (numel(x) == 3)
619
-
% compute the pose
620
-
% convert vector form of pose to SE(3)
621
-
622
-
x = x(:)';
623
-
T = transl([x(1:2) 0]) * trotz( x(3) );
624
-
set(h, 'Matrix', T);
625
-
else
626
-
error('bad pose');
627
-
end
628
-
629
-
if nargout > 0
630
-
h_ = h;
631
-
end
632
-
end
633
-
634
-
end % static methods
635
-
636
-
end % classdef
+1
-4
Assignment_3/code/matlab/Differential.m
+1
-4
Assignment_3/code/matlab/Differential.m
···
235
235
236
236
% calculate speed
237
237
vx = vxScale * (r / 2) * (uR + uL); %% velocity
238
-
uvx = 20 * vx;
239
238
omega = omegaScale * (r / L) * (uR - uL); %% twist
240
239
241
240
% update new state using Euler integration, not expmap
···
246
245
R = vx;
247
246
end
248
247
dx = [vx; 0] + R * [sin(omega * veh.dt); 1 - cos(omega * veh.dt)];
249
-
250
-
pause(0.25);
251
248
252
249
theta = xp(3);
253
250
rot = [cos(theta), -sin(theta);
···
259
256
veh.x(2) = xp(2) + dxw(2); %% new y position
260
257
veh.x(3) = xp(3) + omega * veh.dt; %% new theta rotation
261
258
262
-
odo = [uvx*veh.dt omega*veh.dt]
259
+
odo = [norm(dxw), omega*veh.dt];
263
260
veh.odometry = odo;
264
261
265
262
veh.x_hist = [veh.x_hist; veh.x']; % maintain history
+3
Assignment_3/code/matlab/GenericParticleFilter.m
+3
Assignment_3/code/matlab/GenericParticleFilter.m
+9
-6
Assignment_3/code/matlab/RangeSensor.m
+9
-6
Assignment_3/code/matlab/RangeSensor.m
···
206
206
if s.verbose
207
207
fprintf('Sensor:: feature %d: %.1f %.1f\n', k, z);
208
208
end
209
-
if ~isempty(z) & s.animate
209
+
if ~isempty(z) && s.animate
210
210
s.plot(jf);
211
211
end
212
212
end
···
278
278
% % The weight is never zero.
279
279
% end
280
280
281
-
weights = zeros(length(x),1);
281
+
282
+
weights = ones(length(x),1);
282
283
283
-
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
284
-
% TODO: Implement this function!
285
-
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
286
-
284
+
for i = 1 : length(x)
285
+
z_actual = z;
286
+
z_pred = s.h(x(i,:)');
287
+
z_diff = z_actual - z_pred;
288
+
weights(i) = exp(-0.5 * z_diff ^ 2 * L);
289
+
end
287
290
end
288
291
289
292
function str = char(s)
-37
Assignment_3/code/matlab/script01_MotionModel.asv
-37
Assignment_3/code/matlab/script01_MotionModel.asv
···
1
-
% demo08_squareWorldMCL
2
-
% Demo generic Monte Carlo Localization
3
-
4
-
path(path,'threedee')
5
-
6
-
%% Create a map
7
-
A = [
8
-
0, 0, 15.5, 11.5
9
-
15.5, 11.5, 20, 11
10
-
45.5, 0, 6, 36.5
11
-
0, 21, 5.5, 15.5
12
-
5.5, 36.5, 22.5, 4.5
13
-
28, 36.5, 17.5, 6
14
-
]
15
-
A(:, 1) = A(:, 1) - 20;
16
-
A(:, 2) = A(:, 2) - 20;
17
-
map=SquareMap(A, 35)
18
-
19
-
%% and a robot with noisy odometry
20
-
V=diag([0.01, 0.1*pi/180].^2)
21
-
veh=Differential(V, 'x0', [-20, 0, 0])
22
-
veh.add_driver(DeterministicPath('log-1423005664.txt'));
23
-
24
-
%% and then a sensor with noisy readings
25
-
W=0.05^2;
26
-
sensor = RangeSensor(veh,map, W,'log-1423005664.txt')
27
-
28
-
%% define two covariances for random noise Q and L (hmmm!)
29
-
% For Q, use the uncertainly estimates from A2!
30
-
Q = diag([0.01,0.01,0.1*pi/180].^2);
31
-
L = diag(0.1);
32
-
33
-
%% Finally, construct ParticleFilter
34
-
pf = GenericParticleFilter(veh, sensor, Q, L, 200);
35
-
36
-
%% and run for 1000 steps
37
-
pf.run(1,'nouniform','nosense');
+2
-3
Assignment_3/code/matlab/script02_SensorModel.m
+2
-3
Assignment_3/code/matlab/script02_SensorModel.m
···
1
1
% demo08_squareWorldMCL
2
2
% Demo generic Monte Carlo Localization
3
-
4
3
path(path,'threedee')
5
4
6
5
%% Create a map
···
18
17
19
18
%% and a robot with noisy odometry
20
19
V=diag([0.01, 0.1*pi/180].^2)
21
-
veh=Differential(V, 'x0', [-20, 0, 0])
20
+
veh=Differential(V, 'x0', [-20, -3, 0])
22
21
veh.add_driver(DeterministicPath('log-1423005664.txt'));
23
22
24
23
%% and then a sensor with noisy readings
···
34
33
pf = GenericParticleFilter(veh, sensor, Q, L, 200);
35
34
36
35
%% and run for 1000 steps
37
-
pf.run(1000,'nouniform');
36
+
pf.run(110,'nouniform');
38
37