Newer
Older
5001
5002
5003
5004
5005
5006
5007
5008
5009
5010
5011
5012
5013
5014
5015
5016
5017
5018
5019
5020
5021
5022
5023
5024
5025
5026
5027
5028
5029
5030
5031
5032
5033
5034
5035
5036
5037
5038
5039
5040
5041
5042
5043
5044
5045
5046
5047
5048
5049
5050
5051
5052
5053
5054
5055
5056
5057
5058
5059
5060
5061
5062
5063
5064
5065
5066
5067
5068
5069
5070
5071
5072
5073
5074
5075
5076
5077
5078
5079
5080
5081
5082
5083
5084
5085
5086
5087
5088
5089
5090
5091
5092
5093
5094
5095
5096
5097
5098
5099
5100
5101
5102
5103
5104
5105
5106
5107
5108
5109
5110
5111
5112
5113
5114
5115
5116
5117
5118
5119
5120
5121
5122
5123
5124
5125
5126
5127
5128
5129
5130
5131
5132
5133
5134
5135
5136
5137
5138
5139
5140
5141
5142
5143
5144
5145
5146
5147
5148
"<html>\n",
"<head>\n",
" <title></title>\n",
" <meta http-equiv=\"content-type\" content=\"text/html; charset=None\">\n",
" <style type=\"text/css\">\n",
"td.linenos { background-color: #f0f0f0; padding-right: 10px; }\n",
"span.lineno { background-color: #f0f0f0; padding: 0 5px 0 5px; }\n",
"pre { line-height: 125%; }\n",
"body .hll { background-color: #ffffcc }\n",
"body { background: #f8f8f8; }\n",
"body .c { color: #408080; font-style: italic } /* Comment */\n",
"body .err { border: 1px solid #FF0000 } /* Error */\n",
"body .k { color: #008000; font-weight: bold } /* Keyword */\n",
"body .o { color: #666666 } /* Operator */\n",
"body .ch { color: #408080; font-style: italic } /* Comment.Hashbang */\n",
"body .cm { color: #408080; font-style: italic } /* Comment.Multiline */\n",
"body .cp { color: #BC7A00 } /* Comment.Preproc */\n",
"body .cpf { color: #408080; font-style: italic } /* Comment.PreprocFile */\n",
"body .c1 { color: #408080; font-style: italic } /* Comment.Single */\n",
"body .cs { color: #408080; font-style: italic } /* Comment.Special */\n",
"body .gd { color: #A00000 } /* Generic.Deleted */\n",
"body .ge { font-style: italic } /* Generic.Emph */\n",
"body .gr { color: #FF0000 } /* Generic.Error */\n",
"body .gh { color: #000080; font-weight: bold } /* Generic.Heading */\n",
"body .gi { color: #00A000 } /* Generic.Inserted */\n",
"body .go { color: #888888 } /* Generic.Output */\n",
"body .gp { color: #000080; font-weight: bold } /* Generic.Prompt */\n",
"body .gs { font-weight: bold } /* Generic.Strong */\n",
"body .gu { color: #800080; font-weight: bold } /* Generic.Subheading */\n",
"body .gt { color: #0044DD } /* Generic.Traceback */\n",
"body .kc { color: #008000; font-weight: bold } /* Keyword.Constant */\n",
"body .kd { color: #008000; font-weight: bold } /* Keyword.Declaration */\n",
"body .kn { color: #008000; font-weight: bold } /* Keyword.Namespace */\n",
"body .kp { color: #008000 } /* Keyword.Pseudo */\n",
"body .kr { color: #008000; font-weight: bold } /* Keyword.Reserved */\n",
"body .kt { color: #B00040 } /* Keyword.Type */\n",
"body .m { color: #666666 } /* Literal.Number */\n",
"body .s { color: #BA2121 } /* Literal.String */\n",
"body .na { color: #7D9029 } /* Name.Attribute */\n",
"body .nb { color: #008000 } /* Name.Builtin */\n",
"body .nc { color: #0000FF; font-weight: bold } /* Name.Class */\n",
"body .no { color: #880000 } /* Name.Constant */\n",
"body .nd { color: #AA22FF } /* Name.Decorator */\n",
"body .ni { color: #999999; font-weight: bold } /* Name.Entity */\n",
"body .ne { color: #D2413A; font-weight: bold } /* Name.Exception */\n",
"body .nf { color: #0000FF } /* Name.Function */\n",
"body .nl { color: #A0A000 } /* Name.Label */\n",
"body .nn { color: #0000FF; font-weight: bold } /* Name.Namespace */\n",
"body .nt { color: #008000; font-weight: bold } /* Name.Tag */\n",
"body .nv { color: #19177C } /* Name.Variable */\n",
"body .ow { color: #AA22FF; font-weight: bold } /* Operator.Word */\n",
"body .w { color: #bbbbbb } /* Text.Whitespace */\n",
"body .mb { color: #666666 } /* Literal.Number.Bin */\n",
"body .mf { color: #666666 } /* Literal.Number.Float */\n",
"body .mh { color: #666666 } /* Literal.Number.Hex */\n",
"body .mi { color: #666666 } /* Literal.Number.Integer */\n",
"body .mo { color: #666666 } /* Literal.Number.Oct */\n",
"body .sa { color: #BA2121 } /* Literal.String.Affix */\n",
"body .sb { color: #BA2121 } /* Literal.String.Backtick */\n",
"body .sc { color: #BA2121 } /* Literal.String.Char */\n",
"body .dl { color: #BA2121 } /* Literal.String.Delimiter */\n",
"body .sd { color: #BA2121; font-style: italic } /* Literal.String.Doc */\n",
"body .s2 { color: #BA2121 } /* Literal.String.Double */\n",
"body .se { color: #BB6622; font-weight: bold } /* Literal.String.Escape */\n",
"body .sh { color: #BA2121 } /* Literal.String.Heredoc */\n",
"body .si { color: #BB6688; font-weight: bold } /* Literal.String.Interpol */\n",
"body .sx { color: #008000 } /* Literal.String.Other */\n",
"body .sr { color: #BB6688 } /* Literal.String.Regex */\n",
"body .s1 { color: #BA2121 } /* Literal.String.Single */\n",
"body .ss { color: #19177C } /* Literal.String.Symbol */\n",
"body .bp { color: #008000 } /* Name.Builtin.Pseudo */\n",
"body .fm { color: #0000FF } /* Name.Function.Magic */\n",
"body .vc { color: #19177C } /* Name.Variable.Class */\n",
"body .vg { color: #19177C } /* Name.Variable.Global */\n",
"body .vi { color: #19177C } /* Name.Variable.Instance */\n",
"body .vm { color: #19177C } /* Name.Variable.Magic */\n",
"body .il { color: #666666 } /* Literal.Number.Integer.Long */\n",
"\n",
" </style>\n",
"</head>\n",
"<body>\n",
"<h2></h2>\n",
"\n",
"<div class=\"highlight\"><pre><span></span><span class=\"k\">def</span> <span class=\"nf\">fixed_lag_smoothing</span><span class=\"p\">(</span><span class=\"n\">e_t</span><span class=\"p\">,</span> <span class=\"n\">HMM</span><span class=\"p\">,</span> <span class=\"n\">d</span><span class=\"p\">,</span> <span class=\"n\">ev</span><span class=\"p\">,</span> <span class=\"n\">t</span><span class=\"p\">):</span>\n",
" <span class=\"sd\">"""[Figure 15.6]</span>\n",
"<span class=\"sd\"> Smoothing algorithm with a fixed time lag of 'd' steps.</span>\n",
"<span class=\"sd\"> Online algorithm that outputs the new smoothed estimate if observation</span>\n",
"<span class=\"sd\"> for new time step is given."""</span>\n",
" <span class=\"n\">ev</span><span class=\"o\">.</span><span class=\"n\">insert</span><span class=\"p\">(</span><span class=\"mi\">0</span><span class=\"p\">,</span> <span class=\"bp\">None</span><span class=\"p\">)</span>\n",
"\n",
" <span class=\"n\">T_model</span> <span class=\"o\">=</span> <span class=\"n\">HMM</span><span class=\"o\">.</span><span class=\"n\">transition_model</span>\n",
" <span class=\"n\">f</span> <span class=\"o\">=</span> <span class=\"n\">HMM</span><span class=\"o\">.</span><span class=\"n\">prior</span>\n",
" <span class=\"n\">B</span> <span class=\"o\">=</span> <span class=\"p\">[[</span><span class=\"mi\">1</span><span class=\"p\">,</span> <span class=\"mi\">0</span><span class=\"p\">],</span> <span class=\"p\">[</span><span class=\"mi\">0</span><span class=\"p\">,</span> <span class=\"mi\">1</span><span class=\"p\">]]</span>\n",
" <span class=\"n\">evidence</span> <span class=\"o\">=</span> <span class=\"p\">[]</span>\n",
"\n",
" <span class=\"n\">evidence</span><span class=\"o\">.</span><span class=\"n\">append</span><span class=\"p\">(</span><span class=\"n\">e_t</span><span class=\"p\">)</span>\n",
" <span class=\"n\">O_t</span> <span class=\"o\">=</span> <span class=\"n\">vector_to_diagonal</span><span class=\"p\">(</span><span class=\"n\">HMM</span><span class=\"o\">.</span><span class=\"n\">sensor_dist</span><span class=\"p\">(</span><span class=\"n\">e_t</span><span class=\"p\">))</span>\n",
" <span class=\"k\">if</span> <span class=\"n\">t</span> <span class=\"o\">></span> <span class=\"n\">d</span><span class=\"p\">:</span>\n",
" <span class=\"n\">f</span> <span class=\"o\">=</span> <span class=\"n\">forward</span><span class=\"p\">(</span><span class=\"n\">HMM</span><span class=\"p\">,</span> <span class=\"n\">f</span><span class=\"p\">,</span> <span class=\"n\">e_t</span><span class=\"p\">)</span>\n",
" <span class=\"n\">O_tmd</span> <span class=\"o\">=</span> <span class=\"n\">vector_to_diagonal</span><span class=\"p\">(</span><span class=\"n\">HMM</span><span class=\"o\">.</span><span class=\"n\">sensor_dist</span><span class=\"p\">(</span><span class=\"n\">ev</span><span class=\"p\">[</span><span class=\"n\">t</span> <span class=\"o\">-</span> <span class=\"n\">d</span><span class=\"p\">]))</span>\n",
" <span class=\"n\">B</span> <span class=\"o\">=</span> <span class=\"n\">matrix_multiplication</span><span class=\"p\">(</span><span class=\"n\">inverse_matrix</span><span class=\"p\">(</span><span class=\"n\">O_tmd</span><span class=\"p\">),</span> <span class=\"n\">inverse_matrix</span><span class=\"p\">(</span><span class=\"n\">T_model</span><span class=\"p\">),</span> <span class=\"n\">B</span><span class=\"p\">,</span> <span class=\"n\">T_model</span><span class=\"p\">,</span> <span class=\"n\">O_t</span><span class=\"p\">)</span>\n",
" <span class=\"k\">else</span><span class=\"p\">:</span>\n",
" <span class=\"n\">B</span> <span class=\"o\">=</span> <span class=\"n\">matrix_multiplication</span><span class=\"p\">(</span><span class=\"n\">B</span><span class=\"p\">,</span> <span class=\"n\">T_model</span><span class=\"p\">,</span> <span class=\"n\">O_t</span><span class=\"p\">)</span>\n",
" <span class=\"n\">t</span> <span class=\"o\">+=</span> <span class=\"mi\">1</span>\n",
"\n",
" <span class=\"k\">if</span> <span class=\"n\">t</span> <span class=\"o\">></span> <span class=\"n\">d</span><span class=\"p\">:</span>\n",
" <span class=\"c1\"># always returns a 1x2 matrix</span>\n",
" <span class=\"k\">return</span> <span class=\"p\">[</span><span class=\"n\">normalize</span><span class=\"p\">(</span><span class=\"n\">i</span><span class=\"p\">)</span> <span class=\"k\">for</span> <span class=\"n\">i</span> <span class=\"ow\">in</span> <span class=\"n\">matrix_multiplication</span><span class=\"p\">([</span><span class=\"n\">f</span><span class=\"p\">],</span> <span class=\"n\">B</span><span class=\"p\">)][</span><span class=\"mi\">0</span><span class=\"p\">]</span>\n",
" <span class=\"k\">else</span><span class=\"p\">:</span>\n",
" <span class=\"k\">return</span> <span class=\"bp\">None</span>\n",
"</pre></div>\n",
"</body>\n",
"</html>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"psource(fixed_lag_smoothing)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"This algorithm applies `forward` as usual and optimizes the smoothing step by using the equations above.\n",
"This optimization could be achieved only because HMM properties can be represented as matrices.\n",
"<br>\n",
"`vector_to_diagonal`, `matrix_multiplication` and `inverse_matrix` are matrix manipulation functions to simplify the implementation.\n",
"<br>\n",
"`normalize` is used to normalize the output before returning it."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here's how we can use `fixed_lag_smoothing` for inference on our umbrella HMM."
]
},
{
"cell_type": "code",
"execution_count": 82,
5152
5153
5154
5155
5156
5157
5158
5159
5160
5161
5162
5163
5164
5165
5166
5167
5168
5169
5170
5171
5172
5173
5174
5175
5176
5177
5178
5179
5180
5181
5182
5183
5184
5185
5186
5187
5188
5189
5190
5191
5192
5193
5194
5195
5196
5197
5198
5199
5200
5201
5202
5203
5204
5205
5206
5207
5208
5209
5210
5211
5212
5213
5214
5215
5216
5217
5218
5219
5220
5221
"outputs": [],
"source": [
"umbrella_transition_model = [[0.7, 0.3], [0.3, 0.7]]\n",
"umbrella_sensor_model = [[0.9, 0.2], [0.1, 0.8]]\n",
"hmm = HiddenMarkovModel(umbrella_transition_model, umbrella_sensor_model)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Given evidence T, F, T, F and T, we want to calculate the probability distribution for the fourth day with a fixed lag of 2 days.\n",
"<br>\n",
"Let `e_t = False`"
]
},
{
"cell_type": "code",
"execution_count": 83,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"[0.1111111111111111, 0.8888888888888888]"
]
},
"execution_count": 83,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"e_t = F\n",
"evidence = [T, F, T, F, T]\n",
"fixed_lag_smoothing(e_t, hmm, d=2, ev=evidence, t=4)"
]
},
{
"cell_type": "code",
"execution_count": 84,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"[0.9938650306748466, 0.006134969325153394]"
]
},
"execution_count": 84,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"e_t = T\n",
"evidence = [T, T, F, T, T]\n",
"fixed_lag_smoothing(e_t, hmm, d=1, ev=evidence, t=4)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We cannot calculate probability distributions when $t$ is less than $d$"
]
},
{
"cell_type": "code",
"execution_count": 85,
5225
5226
5227
5228
5229
5230
5231
5232
5233
5234
5235
5236
5237
5238
5239
5240
5241
5242
5243
5244
5245
5246
5247
5248
5249
5250
5251
5252
5253
5254
5255
5256
5257
5258
5259
5260
5261
5262
5263
5264
5265
5266
5267
5268
5269
5270
5271
5272
5273
5274
5275
5276
5277
5278
5279
5280
5281
5282
5283
5284
5285
5286
5287
5288
5289
5290
5291
5292
5293
5294
5295
5296
5297
5298
5299
5300
5301
5302
5303
5304
5305
5306
5307
5308
5309
5310
5311
5312
5313
5314
5315
5316
5317
5318
5319
5320
5321
5322
5323
5324
5325
5326
5327
5328
5329
5330
5331
5332
5333
5334
5335
5336
5337
5338
5339
5340
5341
5342
5343
5344
5345
5346
5347
5348
5349
5350
5351
5352
5353
5354
5355
5356
5357
5358
5359
5360
5361
5362
5363
5364
5365
5366
5367
5368
5369
5370
5371
5372
5373
5374
5375
5376
5377
5378
5379
5380
5381
5382
5383
5384
5385
5386
5387
5388
5389
5390
5391
5392
5393
5394
5395
5396
5397
5398
5399
5400
5401
5402
5403
5404
5405
5406
5407
5408
5409
5410
5411
5412
5413
5414
5415
5416
5417
5418
5419
5420
5421
5422
5423
5424
5425
5426
5427
5428
5429
5430
5431
5432
5433
5434
5435
5436
5437
5438
5439
5440
5441
5442
5443
5444
5445
5446
5447
5448
5449
5450
5451
5452
"outputs": [],
"source": [
"fixed_lag_smoothing(e_t, hmm, d=5, ev=evidence, t=4)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"As expected, the output is `None`"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### PARTICLE FILTERING\n",
"The filtering problem is too expensive to solve using the previous methods for problems with large or continuous state spaces.\n",
"Particle filtering is a method that can solve the same problem but when the state space is a lot larger, where we wouldn't be able to do these computations in a reasonable amount of time as fast, as time goes by, and we want to keep track of things as they happen.\n",
"<br>\n",
"The downside is that it is a sampling method and hence isn't accurate, but the more samples we're willing to take, the more accurate we'd get.\n",
"<br>\n",
"In this method, instead of keping track of the probability distribution, we will drop particles in a similar proportion at the required regions.\n",
"The internal representation of this distribution is usually a list of particles with coordinates in the state-space.\n",
"A particle is just a new name for a sample.\n",
"\n",
"Particle filtering can be divided into four steps:\n",
"1. __Initialization__: \n",
"If we have some idea about the prior probability distribution, we drop the initial particles accordingly, or else we just drop them uniformly over the state space.\n",
"\n",
"2. __Forward pass__: \n",
"As time goes by and measurements come in, we are going to move the selected particles into the grid squares that makes the most sense in terms of representing the distribution that we are trying to track.\n",
"When time goes by, we just loop through all our particles and try to simulate what could happen to each one of them by sampling its next position from the transition model.\n",
"This is like prior sampling - samples' frequencies reflect the transition probabilities.\n",
"If we have enough samples we are pretty close to exact values.\n",
"We work through the list of particles, one particle at a time, all we do is stochastically simulate what the outcome might be.\n",
"If we had no dimension of time, and we had no new measurements come in, this would be exactly the same as what we did in prior sampling.\n",
"\n",
"3. __Reweight__:\n",
"As observations come in, don't sample the observations, fix them and downweight the samples based on the evidence just like in likelihood weighting.\n",
"$$w(x) = P(e/x)$$\n",
"$$B(X) \\propto P(e/X)B'(X)$$\n",
"<br>\n",
"As before, the probabilities don't sum to one, since most have been downweighted.\n",
"They sum to an approximation of $P(e)$.\n",
"To normalize the resulting distribution, we can divide by $P(e)$\n",
"<br>\n",
"Likelihood weighting wasn't the best thing for Bayesian networks, because we were not accounting for the incoming evidence so we were getting samples from the prior distribution, in some sense not the right distribution, so we might end up with a lot of particles with low weights. \n",
"These samples were very uninformative and the way we fixed it then was by using __Gibbs sampling__.\n",
"Theoretically, Gibbs sampling can be run on a HMM, but as we iterated over the process infinitely many times in a Bayesian network, we cannot do that here as we have new incoming evidence and we also need computational cycles to propagate through time.\n",
"<br>\n",
"A lot of samples with very low weight and they are not representative of the _actual probability distribution_.\n",
"So if we keep running likelihood weighting, we keep propagating the samples with smaller weights and carry out computations for that even though these samples have no significant contribution to the actual probability distribution.\n",
"Which is why we require this last step.\n",
"\n",
"4. __Resample__:\n",
"Rather than tracking weighted samples, we _resample_.\n",
"We choose from our weighted sample distribution as many times as the number of particles we initially had and we replace these particles too, so that we have a constant number of particles.\n",
"This is equivalent to renormalizing the distribution.\n",
"The samples with low weight are rarely chosen in the new distribution after resampling.\n",
"This newer set of particles after resampling is in some sense more representative of the actual distribution and so we are better allocating our computational cycles.\n",
"Now the update is complete for this time step, continue with the next one.\n",
"\n",
"<br>\n",
"Let's see how this is implemented in the module."
]
},
{
"cell_type": "code",
"execution_count": 86,
"metadata": {},
"outputs": [
{
"data": {
"text/html": [
"<!DOCTYPE html PUBLIC \"-//W3C//DTD HTML 4.01//EN\"\n",
" \"http://www.w3.org/TR/html4/strict.dtd\">\n",
"\n",
"<html>\n",
"<head>\n",
" <title></title>\n",
" <meta http-equiv=\"content-type\" content=\"text/html; charset=None\">\n",
" <style type=\"text/css\">\n",
"td.linenos { background-color: #f0f0f0; padding-right: 10px; }\n",
"span.lineno { background-color: #f0f0f0; padding: 0 5px 0 5px; }\n",
"pre { line-height: 125%; }\n",
"body .hll { background-color: #ffffcc }\n",
"body { background: #f8f8f8; }\n",
"body .c { color: #408080; font-style: italic } /* Comment */\n",
"body .err { border: 1px solid #FF0000 } /* Error */\n",
"body .k { color: #008000; font-weight: bold } /* Keyword */\n",
"body .o { color: #666666 } /* Operator */\n",
"body .ch { color: #408080; font-style: italic } /* Comment.Hashbang */\n",
"body .cm { color: #408080; font-style: italic } /* Comment.Multiline */\n",
"body .cp { color: #BC7A00 } /* Comment.Preproc */\n",
"body .cpf { color: #408080; font-style: italic } /* Comment.PreprocFile */\n",
"body .c1 { color: #408080; font-style: italic } /* Comment.Single */\n",
"body .cs { color: #408080; font-style: italic } /* Comment.Special */\n",
"body .gd { color: #A00000 } /* Generic.Deleted */\n",
"body .ge { font-style: italic } /* Generic.Emph */\n",
"body .gr { color: #FF0000 } /* Generic.Error */\n",
"body .gh { color: #000080; font-weight: bold } /* Generic.Heading */\n",
"body .gi { color: #00A000 } /* Generic.Inserted */\n",
"body .go { color: #888888 } /* Generic.Output */\n",
"body .gp { color: #000080; font-weight: bold } /* Generic.Prompt */\n",
"body .gs { font-weight: bold } /* Generic.Strong */\n",
"body .gu { color: #800080; font-weight: bold } /* Generic.Subheading */\n",
"body .gt { color: #0044DD } /* Generic.Traceback */\n",
"body .kc { color: #008000; font-weight: bold } /* Keyword.Constant */\n",
"body .kd { color: #008000; font-weight: bold } /* Keyword.Declaration */\n",
"body .kn { color: #008000; font-weight: bold } /* Keyword.Namespace */\n",
"body .kp { color: #008000 } /* Keyword.Pseudo */\n",
"body .kr { color: #008000; font-weight: bold } /* Keyword.Reserved */\n",
"body .kt { color: #B00040 } /* Keyword.Type */\n",
"body .m { color: #666666 } /* Literal.Number */\n",
"body .s { color: #BA2121 } /* Literal.String */\n",
"body .na { color: #7D9029 } /* Name.Attribute */\n",
"body .nb { color: #008000 } /* Name.Builtin */\n",
"body .nc { color: #0000FF; font-weight: bold } /* Name.Class */\n",
"body .no { color: #880000 } /* Name.Constant */\n",
"body .nd { color: #AA22FF } /* Name.Decorator */\n",
"body .ni { color: #999999; font-weight: bold } /* Name.Entity */\n",
"body .ne { color: #D2413A; font-weight: bold } /* Name.Exception */\n",
"body .nf { color: #0000FF } /* Name.Function */\n",
"body .nl { color: #A0A000 } /* Name.Label */\n",
"body .nn { color: #0000FF; font-weight: bold } /* Name.Namespace */\n",
"body .nt { color: #008000; font-weight: bold } /* Name.Tag */\n",
"body .nv { color: #19177C } /* Name.Variable */\n",
"body .ow { color: #AA22FF; font-weight: bold } /* Operator.Word */\n",
"body .w { color: #bbbbbb } /* Text.Whitespace */\n",
"body .mb { color: #666666 } /* Literal.Number.Bin */\n",
"body .mf { color: #666666 } /* Literal.Number.Float */\n",
"body .mh { color: #666666 } /* Literal.Number.Hex */\n",
"body .mi { color: #666666 } /* Literal.Number.Integer */\n",
"body .mo { color: #666666 } /* Literal.Number.Oct */\n",
"body .sa { color: #BA2121 } /* Literal.String.Affix */\n",
"body .sb { color: #BA2121 } /* Literal.String.Backtick */\n",
"body .sc { color: #BA2121 } /* Literal.String.Char */\n",
"body .dl { color: #BA2121 } /* Literal.String.Delimiter */\n",
"body .sd { color: #BA2121; font-style: italic } /* Literal.String.Doc */\n",
"body .s2 { color: #BA2121 } /* Literal.String.Double */\n",
"body .se { color: #BB6622; font-weight: bold } /* Literal.String.Escape */\n",
"body .sh { color: #BA2121 } /* Literal.String.Heredoc */\n",
"body .si { color: #BB6688; font-weight: bold } /* Literal.String.Interpol */\n",
"body .sx { color: #008000 } /* Literal.String.Other */\n",
"body .sr { color: #BB6688 } /* Literal.String.Regex */\n",
"body .s1 { color: #BA2121 } /* Literal.String.Single */\n",
"body .ss { color: #19177C } /* Literal.String.Symbol */\n",
"body .bp { color: #008000 } /* Name.Builtin.Pseudo */\n",
"body .fm { color: #0000FF } /* Name.Function.Magic */\n",
"body .vc { color: #19177C } /* Name.Variable.Class */\n",
"body .vg { color: #19177C } /* Name.Variable.Global */\n",
"body .vi { color: #19177C } /* Name.Variable.Instance */\n",
"body .vm { color: #19177C } /* Name.Variable.Magic */\n",
"body .il { color: #666666 } /* Literal.Number.Integer.Long */\n",
"\n",
" </style>\n",
"</head>\n",
"<body>\n",
"<h2></h2>\n",
"\n",
"<div class=\"highlight\"><pre><span></span><span class=\"k\">def</span> <span class=\"nf\">particle_filtering</span><span class=\"p\">(</span><span class=\"n\">e</span><span class=\"p\">,</span> <span class=\"n\">N</span><span class=\"p\">,</span> <span class=\"n\">HMM</span><span class=\"p\">):</span>\n",
" <span class=\"sd\">"""Particle filtering considering two states variables."""</span>\n",
" <span class=\"n\">dist</span> <span class=\"o\">=</span> <span class=\"p\">[</span><span class=\"mf\">0.5</span><span class=\"p\">,</span> <span class=\"mf\">0.5</span><span class=\"p\">]</span>\n",
" <span class=\"c1\"># Weight Initialization</span>\n",
" <span class=\"n\">w</span> <span class=\"o\">=</span> <span class=\"p\">[</span><span class=\"mi\">0</span> <span class=\"k\">for</span> <span class=\"n\">_</span> <span class=\"ow\">in</span> <span class=\"nb\">range</span><span class=\"p\">(</span><span class=\"n\">N</span><span class=\"p\">)]</span>\n",
" <span class=\"c1\"># STEP 1</span>\n",
" <span class=\"c1\"># Propagate one step using transition model given prior state</span>\n",
" <span class=\"n\">dist</span> <span class=\"o\">=</span> <span class=\"n\">vector_add</span><span class=\"p\">(</span><span class=\"n\">scalar_vector_product</span><span class=\"p\">(</span><span class=\"n\">dist</span><span class=\"p\">[</span><span class=\"mi\">0</span><span class=\"p\">],</span> <span class=\"n\">HMM</span><span class=\"o\">.</span><span class=\"n\">transition_model</span><span class=\"p\">[</span><span class=\"mi\">0</span><span class=\"p\">]),</span>\n",
" <span class=\"n\">scalar_vector_product</span><span class=\"p\">(</span><span class=\"n\">dist</span><span class=\"p\">[</span><span class=\"mi\">1</span><span class=\"p\">],</span> <span class=\"n\">HMM</span><span class=\"o\">.</span><span class=\"n\">transition_model</span><span class=\"p\">[</span><span class=\"mi\">1</span><span class=\"p\">]))</span>\n",
" <span class=\"c1\"># Assign state according to probability</span>\n",
" <span class=\"n\">s</span> <span class=\"o\">=</span> <span class=\"p\">[</span><span class=\"s1\">'A'</span> <span class=\"k\">if</span> <span class=\"n\">probability</span><span class=\"p\">(</span><span class=\"n\">dist</span><span class=\"p\">[</span><span class=\"mi\">0</span><span class=\"p\">])</span> <span class=\"k\">else</span> <span class=\"s1\">'B'</span> <span class=\"k\">for</span> <span class=\"n\">_</span> <span class=\"ow\">in</span> <span class=\"nb\">range</span><span class=\"p\">(</span><span class=\"n\">N</span><span class=\"p\">)]</span>\n",
" <span class=\"n\">w_tot</span> <span class=\"o\">=</span> <span class=\"mi\">0</span>\n",
" <span class=\"c1\"># Calculate importance weight given evidence e</span>\n",
" <span class=\"k\">for</span> <span class=\"n\">i</span> <span class=\"ow\">in</span> <span class=\"nb\">range</span><span class=\"p\">(</span><span class=\"n\">N</span><span class=\"p\">):</span>\n",
" <span class=\"k\">if</span> <span class=\"n\">s</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]</span> <span class=\"o\">==</span> <span class=\"s1\">'A'</span><span class=\"p\">:</span>\n",
" <span class=\"c1\"># P(U|A)*P(A)</span>\n",
" <span class=\"n\">w_i</span> <span class=\"o\">=</span> <span class=\"n\">HMM</span><span class=\"o\">.</span><span class=\"n\">sensor_dist</span><span class=\"p\">(</span><span class=\"n\">e</span><span class=\"p\">)[</span><span class=\"mi\">0</span><span class=\"p\">]</span> <span class=\"o\">*</span> <span class=\"n\">dist</span><span class=\"p\">[</span><span class=\"mi\">0</span><span class=\"p\">]</span>\n",
" <span class=\"k\">if</span> <span class=\"n\">s</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]</span> <span class=\"o\">==</span> <span class=\"s1\">'B'</span><span class=\"p\">:</span>\n",
" <span class=\"c1\"># P(U|B)*P(B)</span>\n",
" <span class=\"n\">w_i</span> <span class=\"o\">=</span> <span class=\"n\">HMM</span><span class=\"o\">.</span><span class=\"n\">sensor_dist</span><span class=\"p\">(</span><span class=\"n\">e</span><span class=\"p\">)[</span><span class=\"mi\">1</span><span class=\"p\">]</span> <span class=\"o\">*</span> <span class=\"n\">dist</span><span class=\"p\">[</span><span class=\"mi\">1</span><span class=\"p\">]</span>\n",
" <span class=\"n\">w</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]</span> <span class=\"o\">=</span> <span class=\"n\">w_i</span>\n",
" <span class=\"n\">w_tot</span> <span class=\"o\">+=</span> <span class=\"n\">w_i</span>\n",
"\n",
" <span class=\"c1\"># Normalize all the weights</span>\n",
" <span class=\"k\">for</span> <span class=\"n\">i</span> <span class=\"ow\">in</span> <span class=\"nb\">range</span><span class=\"p\">(</span><span class=\"n\">N</span><span class=\"p\">):</span>\n",
" <span class=\"n\">w</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]</span> <span class=\"o\">=</span> <span class=\"n\">w</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]</span> <span class=\"o\">/</span> <span class=\"n\">w_tot</span>\n",
"\n",
" <span class=\"c1\"># Limit weights to 4 digits</span>\n",
" <span class=\"k\">for</span> <span class=\"n\">i</span> <span class=\"ow\">in</span> <span class=\"nb\">range</span><span class=\"p\">(</span><span class=\"n\">N</span><span class=\"p\">):</span>\n",
" <span class=\"n\">w</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]</span> <span class=\"o\">=</span> <span class=\"nb\">float</span><span class=\"p\">(</span><span class=\"s2\">"{0:.4f}"</span><span class=\"o\">.</span><span class=\"n\">format</span><span class=\"p\">(</span><span class=\"n\">w</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]))</span>\n",
"\n",
" <span class=\"c1\"># STEP 2</span>\n",
"\n",
" <span class=\"n\">s</span> <span class=\"o\">=</span> <span class=\"n\">weighted_sample_with_replacement</span><span class=\"p\">(</span><span class=\"n\">N</span><span class=\"p\">,</span> <span class=\"n\">s</span><span class=\"p\">,</span> <span class=\"n\">w</span><span class=\"p\">)</span>\n",
"\n",
" <span class=\"k\">return</span> <span class=\"n\">s</span>\n",
"</pre></div>\n",
"</body>\n",
"</html>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"psource(particle_filtering)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here, `scalar_vector_product` and `vector_add` are helper functions to help with vector math and `weighted_sample_with_replacement` resamples from a weighted sample and replaces the original sample, as is obvious from the name.\n",
"<br>\n",
"This implementation considers two state variables with generic names 'A' and 'B'.\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Here's how we can use `particle_filtering` on our umbrella HMM, though it doesn't make much sense using particle filtering on a problem with such a small state space.\n",
"It is just to get familiar with the syntax."
]
},
{
"cell_type": "code",
"execution_count": 87,
"outputs": [],
"source": [
"umbrella_transition_model = [[0.7, 0.3], [0.3, 0.7]]\n",
"umbrella_sensor_model = [[0.9, 0.2], [0.1, 0.8]]\n",
"hmm = HiddenMarkovModel(umbrella_transition_model, umbrella_sensor_model)"
]
},
{
"cell_type": "code",
"execution_count": 88,
"scrolled": false
"outputs": [
{
"data": {
"text/plain": [
"['A', 'A', 'A', 'A', 'B', 'A', 'B', 'B', 'B', 'B']"
]
},
"execution_count": 88,
"metadata": {},
"output_type": "execute_result"
}
],
"particle_filtering(T, 10, hmm)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We got 5 samples from state `A` and 5 samples from state `B`"
]
},
{
"cell_type": "code",
"execution_count": 89,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"['A', 'B', 'B', 'B', 'B', 'B', 'B', 'B', 'A', 'B']"
]
},
"execution_count": 89,
"metadata": {},
"output_type": "execute_result"
}
],
"particle_filtering([F, T, F, F, T], 10, hmm)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
5521
5522
5523
5524
5525
5526
5527
5528
5529
5530
5531
5532
5533
5534
5535
5536
5537
5538
5539
5540
5541
5542
5543
5544
5545
5546
5547
5548
5549
5550
5551
5552
5553
5554
5555
5556
5557
5558
5559
5560
5561
5562
5563
5564
5565
5566
5567
5568
5569
5570
5571
"This time we got 2 samples from state `A` and 8 samples from state `B`"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Comparing runtimes for these algorithms will not be useful, as each solves the filtering task efficiently for a different scenario.\n",
"<br>\n",
"`forward_backward` calculates the exact probability distribution.\n",
"<br>\n",
"`fixed_lag_smoothing` calculates an approximate distribution and its runtime will depend on the value of the lag chosen.\n",
"<br>\n",
"`particle_filtering` is an efficient method for approximating distributions for a very large or continuous state space."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## MONTE CARLO LOCALIZATION\n",
"In the domain of robotics, particle filtering is used for _robot localization_.\n",
"__Localization__ is the problem of finding out where things are, in this case, we want to find the position of a robot in a continuous state space.\n",
"<br>\n",
"__Monte Carlo Localization__ is an algorithm for robots to _localize_ using a _particle filter_.\n",
"Given a map of the environment, the algorithm estimates the position and orientation of a robot as it moves and senses the environment.\n",
"<br>\n",
"Initially, particles are distributed uniformly over the state space, ie the robot has no information of where it is and assumes it is equally likely to be at any point in space.\n",
"<br>\n",
"When the robot moves, it analyses the incoming evidence to shift and change the probability to better approximate the probability distribution of its position.\n",
"The particles are then resampled based on their weights.\n",
"<br>\n",
"Gradually, as more evidence comes in, the robot gets better at approximating its location and the particles converge towards the actual position of the robot.\n",
"<br>\n",
"The pose of a robot is defined by its two Cartesian coordinates with values $x$ and $y$ and its direction with value $\\theta$.\n",
"We use the kinematic equations of motion to model a deterministic state prediction.\n",
"This is our motion model (or transition model).\n",
"<br>\n",
"Next, we need a sensor model.\n",
"There can be two kinds of sensor models, the first assumes that the sensors detect _stable_, _recognizable_ features of the environment called __landmarks__.\n",
"The robot senses the location and bearing of each landmark and updates its belief according to that.\n",
"We can also assume the noise in measurements to be Gaussian, to simplify things.\n",
"<br>\n",
"Another kind of sensor model is used for an array of range sensors, each of which has a fixed bearing relative to the robot.\n",
"These sensors provide a set of range values in each direction.\n",
"This will also be corrupted by Gaussian noise, but we can assume that the errors for different beam directions are independent and identically distributed.\n",
"<br>\n",
"After evidence comes in, the robot updates its belief state and reweights the particle distribution to better aproximate the actual distribution.\n",
"<br>\n",
"<br>\n",
"Let's have a look at how this algorithm is implemented in the module"
]
},
{
"cell_type": "code",
5576
5577
5578
5579
5580
5581
5582
5583
5584
5585
5586
5587
5588
5589
5590
5591
5592
5593
5594
5595
5596
5597
5598
5599
5600
5601
5602
5603
5604
5605
5606
5607
5608
5609
5610
5611
5612
5613
5614
5615
5616
5617
5618
5619
5620
5621
5622
5623
5624
5625
5626
5627
5628
5629
5630
5631
5632
5633
5634
5635
5636
5637
5638
5639
5640
5641
5642
5643
5644
5645
5646
5647
5648
5649
5650
5651
5652
5653
5654
5655
5656
5657
5658
5659
5660
5661
5662
5663
5664
5665
5666
5667
5668
5669
5670
5671
5672
5673
5674
5675
5676
5677
5678
5679
5680
5681
5682
5683
5684
5685
5686
5687
5688
5689
5690
5691
5692
5693
5694
5695
5696
5697
5698
5699
5700
5701
5702
5703
5704
"execution_count": 90,
"metadata": {},
"outputs": [
{
"data": {
"text/html": [
"<!DOCTYPE html PUBLIC \"-//W3C//DTD HTML 4.01//EN\"\n",
" \"http://www.w3.org/TR/html4/strict.dtd\">\n",
"\n",
"<html>\n",
"<head>\n",
" <title></title>\n",
" <meta http-equiv=\"content-type\" content=\"text/html; charset=None\">\n",
" <style type=\"text/css\">\n",
"td.linenos { background-color: #f0f0f0; padding-right: 10px; }\n",
"span.lineno { background-color: #f0f0f0; padding: 0 5px 0 5px; }\n",
"pre { line-height: 125%; }\n",
"body .hll { background-color: #ffffcc }\n",
"body { background: #f8f8f8; }\n",
"body .c { color: #408080; font-style: italic } /* Comment */\n",
"body .err { border: 1px solid #FF0000 } /* Error */\n",
"body .k { color: #008000; font-weight: bold } /* Keyword */\n",
"body .o { color: #666666 } /* Operator */\n",
"body .ch { color: #408080; font-style: italic } /* Comment.Hashbang */\n",
"body .cm { color: #408080; font-style: italic } /* Comment.Multiline */\n",
"body .cp { color: #BC7A00 } /* Comment.Preproc */\n",
"body .cpf { color: #408080; font-style: italic } /* Comment.PreprocFile */\n",
"body .c1 { color: #408080; font-style: italic } /* Comment.Single */\n",
"body .cs { color: #408080; font-style: italic } /* Comment.Special */\n",
"body .gd { color: #A00000 } /* Generic.Deleted */\n",
"body .ge { font-style: italic } /* Generic.Emph */\n",
"body .gr { color: #FF0000 } /* Generic.Error */\n",
"body .gh { color: #000080; font-weight: bold } /* Generic.Heading */\n",
"body .gi { color: #00A000 } /* Generic.Inserted */\n",
"body .go { color: #888888 } /* Generic.Output */\n",
"body .gp { color: #000080; font-weight: bold } /* Generic.Prompt */\n",
"body .gs { font-weight: bold } /* Generic.Strong */\n",
"body .gu { color: #800080; font-weight: bold } /* Generic.Subheading */\n",
"body .gt { color: #0044DD } /* Generic.Traceback */\n",
"body .kc { color: #008000; font-weight: bold } /* Keyword.Constant */\n",
"body .kd { color: #008000; font-weight: bold } /* Keyword.Declaration */\n",
"body .kn { color: #008000; font-weight: bold } /* Keyword.Namespace */\n",
"body .kp { color: #008000 } /* Keyword.Pseudo */\n",
"body .kr { color: #008000; font-weight: bold } /* Keyword.Reserved */\n",
"body .kt { color: #B00040 } /* Keyword.Type */\n",
"body .m { color: #666666 } /* Literal.Number */\n",
"body .s { color: #BA2121 } /* Literal.String */\n",
"body .na { color: #7D9029 } /* Name.Attribute */\n",
"body .nb { color: #008000 } /* Name.Builtin */\n",
"body .nc { color: #0000FF; font-weight: bold } /* Name.Class */\n",
"body .no { color: #880000 } /* Name.Constant */\n",
"body .nd { color: #AA22FF } /* Name.Decorator */\n",
"body .ni { color: #999999; font-weight: bold } /* Name.Entity */\n",
"body .ne { color: #D2413A; font-weight: bold } /* Name.Exception */\n",
"body .nf { color: #0000FF } /* Name.Function */\n",
"body .nl { color: #A0A000 } /* Name.Label */\n",
"body .nn { color: #0000FF; font-weight: bold } /* Name.Namespace */\n",
"body .nt { color: #008000; font-weight: bold } /* Name.Tag */\n",
"body .nv { color: #19177C } /* Name.Variable */\n",
"body .ow { color: #AA22FF; font-weight: bold } /* Operator.Word */\n",
"body .w { color: #bbbbbb } /* Text.Whitespace */\n",
"body .mb { color: #666666 } /* Literal.Number.Bin */\n",
"body .mf { color: #666666 } /* Literal.Number.Float */\n",
"body .mh { color: #666666 } /* Literal.Number.Hex */\n",
"body .mi { color: #666666 } /* Literal.Number.Integer */\n",
"body .mo { color: #666666 } /* Literal.Number.Oct */\n",
"body .sa { color: #BA2121 } /* Literal.String.Affix */\n",
"body .sb { color: #BA2121 } /* Literal.String.Backtick */\n",
"body .sc { color: #BA2121 } /* Literal.String.Char */\n",
"body .dl { color: #BA2121 } /* Literal.String.Delimiter */\n",
"body .sd { color: #BA2121; font-style: italic } /* Literal.String.Doc */\n",
"body .s2 { color: #BA2121 } /* Literal.String.Double */\n",
"body .se { color: #BB6622; font-weight: bold } /* Literal.String.Escape */\n",
"body .sh { color: #BA2121 } /* Literal.String.Heredoc */\n",
"body .si { color: #BB6688; font-weight: bold } /* Literal.String.Interpol */\n",
"body .sx { color: #008000 } /* Literal.String.Other */\n",
"body .sr { color: #BB6688 } /* Literal.String.Regex */\n",
"body .s1 { color: #BA2121 } /* Literal.String.Single */\n",
"body .ss { color: #19177C } /* Literal.String.Symbol */\n",
"body .bp { color: #008000 } /* Name.Builtin.Pseudo */\n",
"body .fm { color: #0000FF } /* Name.Function.Magic */\n",
"body .vc { color: #19177C } /* Name.Variable.Class */\n",
"body .vg { color: #19177C } /* Name.Variable.Global */\n",
"body .vi { color: #19177C } /* Name.Variable.Instance */\n",
"body .vm { color: #19177C } /* Name.Variable.Magic */\n",
"body .il { color: #666666 } /* Literal.Number.Integer.Long */\n",
"\n",
" </style>\n",
"</head>\n",
"<body>\n",
"<h2></h2>\n",
"\n",
"<div class=\"highlight\"><pre><span></span><span class=\"k\">def</span> <span class=\"nf\">monte_carlo_localization</span><span class=\"p\">(</span><span class=\"n\">a</span><span class=\"p\">,</span> <span class=\"n\">z</span><span class=\"p\">,</span> <span class=\"n\">N</span><span class=\"p\">,</span> <span class=\"n\">P_motion_sample</span><span class=\"p\">,</span> <span class=\"n\">P_sensor</span><span class=\"p\">,</span> <span class=\"n\">m</span><span class=\"p\">,</span> <span class=\"n\">S</span><span class=\"o\">=</span><span class=\"bp\">None</span><span class=\"p\">):</span>\n",
" <span class=\"sd\">"""Monte Carlo localization algorithm from Fig 25.9"""</span>\n",
"\n",
" <span class=\"k\">def</span> <span class=\"nf\">ray_cast</span><span class=\"p\">(</span><span class=\"n\">sensor_num</span><span class=\"p\">,</span> <span class=\"n\">kin_state</span><span class=\"p\">,</span> <span class=\"n\">m</span><span class=\"p\">):</span>\n",
" <span class=\"k\">return</span> <span class=\"n\">m</span><span class=\"o\">.</span><span class=\"n\">ray_cast</span><span class=\"p\">(</span><span class=\"n\">sensor_num</span><span class=\"p\">,</span> <span class=\"n\">kin_state</span><span class=\"p\">)</span>\n",
"\n",
" <span class=\"n\">M</span> <span class=\"o\">=</span> <span class=\"nb\">len</span><span class=\"p\">(</span><span class=\"n\">z</span><span class=\"p\">)</span>\n",
" <span class=\"n\">W</span> <span class=\"o\">=</span> <span class=\"p\">[</span><span class=\"mi\">0</span><span class=\"p\">]</span><span class=\"o\">*</span><span class=\"n\">N</span>\n",
" <span class=\"n\">S_</span> <span class=\"o\">=</span> <span class=\"p\">[</span><span class=\"mi\">0</span><span class=\"p\">]</span><span class=\"o\">*</span><span class=\"n\">N</span>\n",
" <span class=\"n\">W_</span> <span class=\"o\">=</span> <span class=\"p\">[</span><span class=\"mi\">0</span><span class=\"p\">]</span><span class=\"o\">*</span><span class=\"n\">N</span>\n",
" <span class=\"n\">v</span> <span class=\"o\">=</span> <span class=\"n\">a</span><span class=\"p\">[</span><span class=\"s1\">'v'</span><span class=\"p\">]</span>\n",
" <span class=\"n\">w</span> <span class=\"o\">=</span> <span class=\"n\">a</span><span class=\"p\">[</span><span class=\"s1\">'w'</span><span class=\"p\">]</span>\n",
"\n",
" <span class=\"k\">if</span> <span class=\"n\">S</span> <span class=\"ow\">is</span> <span class=\"bp\">None</span><span class=\"p\">:</span>\n",
" <span class=\"n\">S</span> <span class=\"o\">=</span> <span class=\"p\">[</span><span class=\"n\">m</span><span class=\"o\">.</span><span class=\"n\">sample</span><span class=\"p\">()</span> <span class=\"k\">for</span> <span class=\"n\">_</span> <span class=\"ow\">in</span> <span class=\"nb\">range</span><span class=\"p\">(</span><span class=\"n\">N</span><span class=\"p\">)]</span>\n",
"\n",
" <span class=\"k\">for</span> <span class=\"n\">i</span> <span class=\"ow\">in</span> <span class=\"nb\">range</span><span class=\"p\">(</span><span class=\"n\">N</span><span class=\"p\">):</span>\n",
" <span class=\"n\">S_</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]</span> <span class=\"o\">=</span> <span class=\"n\">P_motion_sample</span><span class=\"p\">(</span><span class=\"n\">S</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">],</span> <span class=\"n\">v</span><span class=\"p\">,</span> <span class=\"n\">w</span><span class=\"p\">)</span>\n",
" <span class=\"n\">W_</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]</span> <span class=\"o\">=</span> <span class=\"mi\">1</span>\n",
" <span class=\"k\">for</span> <span class=\"n\">j</span> <span class=\"ow\">in</span> <span class=\"nb\">range</span><span class=\"p\">(</span><span class=\"n\">M</span><span class=\"p\">):</span>\n",
" <span class=\"n\">z_</span> <span class=\"o\">=</span> <span class=\"n\">ray_cast</span><span class=\"p\">(</span><span class=\"n\">j</span><span class=\"p\">,</span> <span class=\"n\">S_</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">],</span> <span class=\"n\">m</span><span class=\"p\">)</span>\n",
" <span class=\"n\">W_</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]</span> <span class=\"o\">=</span> <span class=\"n\">W_</span><span class=\"p\">[</span><span class=\"n\">i</span><span class=\"p\">]</span> <span class=\"o\">*</span> <span class=\"n\">P_sensor</span><span class=\"p\">(</span><span class=\"n\">z</span><span class=\"p\">[</span><span class=\"n\">j</span><span class=\"p\">],</span> <span class=\"n\">z_</span><span class=\"p\">)</span>\n",
"\n",
" <span class=\"n\">S</span> <span class=\"o\">=</span> <span class=\"n\">weighted_sample_with_replacement</span><span class=\"p\">(</span><span class=\"n\">N</span><span class=\"p\">,</span> <span class=\"n\">S_</span><span class=\"p\">,</span> <span class=\"n\">W_</span><span class=\"p\">)</span>\n",
" <span class=\"k\">return</span> <span class=\"n\">S</span>\n",
"</pre></div>\n",
"</body>\n",
"</html>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"psource(monte_carlo_localization)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Our implementation of Monte Carlo Localization uses the range scan method.\n",
"The `ray_cast` helper function casts rays in different directions and stores the range values.\n",
"<br>\n",
"`a` stores the `v` and `w` components of the robot's velocity.\n",
"<br>\n",
"`z` is a range scan.\n",
"<br>\n",
"`P_motion_sample` is the motion or transition model.\n",
"<br>\n",
"`P_sensor` is the range sensor noise model.\n",
"<br>\n",
"`m` is the 2D map of the environment\n",
"<br>\n",
"`S` is a vector of samples of size N"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We'll now define a simple 2D map to run Monte Carlo Localization on.\n",
"<br>\n",
"Let's say this is the map we want\n",
"<br>"
]
},
{
"cell_type": "code",
"execution_count": 91,
"metadata": {
"scrolled": true
},
"data": {
"image/png": "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\n",
"text/plain": [
"<matplotlib.figure.Figure at 0x232b896be10>"
]
},
"metadata": {},
"output_type": "display_data"
"m = MCLmap([[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 0],\n",
" [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0],\n",
" [1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 0, 1, 1, 0],\n",
" [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0],\n",
" [0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0],\n",
" [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0],\n",
" [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0],\n",
" [0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0],\n",
" [0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],\n",
" [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0],\n",
" [0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 1, 0]])\n",
"\n",
"heatmap(m.m, cmap='binary')"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's define the motion model as a function `P_motion_sample`."
]
},
{
"cell_type": "code",
"execution_count": 92,
"outputs": [],
"source": [
"def P_motion_sample(kin_state, v, w):\n",
" \"\"\"Sample from possible kinematic states.\n",
" Returns from a single element distribution (no uncertainity in motion)\"\"\"\n",
" pos = kin_state[:2]\n",
" orient = kin_state[2]\n",
"\n",
" # for simplicity the robot first rotates and then moves\n",
" orient = (orient + w)%4\n",
" for _ in range(orient):\n",
" v = (v[1], -v[0])\n",
" pos = vector_add(pos, v)\n",
" return pos + (orient,)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Define the sensor model as a function `P_sensor`."
]
},
{
"cell_type": "code",
"execution_count": 93,
"outputs": [],
"source": [
"def P_sensor(x, y):\n",
" \"\"\"Conditional probability for sensor reading\"\"\"\n",
" # Need not be exact probability. Can use a scaled value.\n",
" if x == y:\n",
" return 0.8\n",
" elif abs(x - y) <= 2:\n",
" return 0.05\n",
" else:\n",
" return 0"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Initializing variables."
]
},
{
"cell_type": "code",
"execution_count": 94,
"outputs": [],
"a = {'v': (0, 0), 'w': 0}\n",
"z = (2, 4, 1, 6)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's run `monte_carlo_localization` with these parameters to find a sample distribution S."
]
},
{
"cell_type": "code",
"execution_count": 95,
"outputs": [],
"source": [
"S = monte_carlo_localization(a, z, 1000, P_motion_sample, P_sensor, m)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's plot the values in the sample distribution `S`."
]
},
{
"cell_type": "code",
"execution_count": 96,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"GRID:\n",
" 0 0 9 41 123 12 1 0 0 0 0 0 0 0 0 0 0\n",
" 0 0 0 0 2 107 56 4 0 0 0 0 0 0 0 0 0\n",
" 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
" 0 0 0 5 4 9 2 0 0 0 0 0 0 0 0 0 0\n",
" 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
" 0 0 10 260 135 5 0 0 0 0 0 0 0 0 0 0 0\n",
" 0 0 0 0 5 34 50 0 0 0 0 0 0 0 0 0 0\n",
"79 4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
"26 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0\n",
" 0 0 0 3 2 10 0 0 0 0 0 0 0 0 0 0 0\n",
" 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n"
]
},
"image/png": "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\n",
"<matplotlib.figure.Figure at 0x232b89dda58>"
]
},
"metadata": {},
"output_type": "display_data"
"grid = [[0]*17 for _ in range(11)]\n",
"for x, y, _ in S:\n",
" if 0 <= x < 11 and 0 <= y < 17:\n",
" grid[x][y] += 1\n",
"print(\"GRID:\")\n",
"print_table(grid)\n",
"heatmap(grid, cmap='Oranges')"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The distribution is highly concentrated at `(5, 3)`, but the robot is not very confident about its position as some other cells also have high probability values."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Let's look at another scenario."
]
},
{
"cell_type": "code",
"execution_count": 97,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"GRID:\n",
"0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
"0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0\n",
"0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
"0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
"0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
"0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
"0 0 0 0 0 0 0 999 0 0 0 0 0 0 0 0 0\n",
"0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
"0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
"0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
"0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n"
]
},
"image/png": "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\n",
"<matplotlib.figure.Figure at 0x232b88cdb38>"
]
},
"metadata": {},
"output_type": "display_data"
"a = {'v': (0, 1), 'w': 0}\n",
"z = (2, 3, 5, 7)\n",
"S = monte_carlo_localization(a, z, 1000, P_motion_sample, P_sensor, m, S)\n",
"grid = [[0]*17 for _ in range(11)]\n",
"for x, y, _ in S:\n",
" if 0 <= x < 11 and 0 <= y < 17:\n",
" grid[x][y] += 1\n",
"print(\"GRID:\")\n",
"print_table(grid)\n",
"heatmap(grid, cmap='Oranges')"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In this case, the robot is 99.9% certain that it is at position `(6, 7)`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We now move into the domain of probabilistic decision making.\n",
"<br>\n",
"To make choices between different possible plans in a certain situation in a given environment, an agent must have _preference_ between the possible outcomes of the various plans.\n",
"<br>\n",
"__Utility theory__ is used to represent and reason with preferences.\n",
"The agent prefers states with a higher _utility_.\n",
"While constructing multi-agent systems, one major element in the design is the mechanism the agents use for making decisions about which actions to adopt in order to achieve their goals.\n",
"What is usually required is a mechanism which ensures that the actions adopted lead to benefits for both individual agents, and the community of which they are part.\n",
"The utility of a state is _relative_ to an agent.\n",
"<br>\n",
"Preferences, as expressed by utilities, are combined with probabilities in the general theory of rational decisions called __decision theory__.\n",
"<br>\n",