ベイズフィルタと粒子フィルタの追加問題

問題: 部屋がA, B, C, Dと4つあり、ロボットは最初部屋Aにいる。 そこで「停留」コマンドを選択した。このコマンドによりロボットは確率 1/2で同じ部屋にとどまるが、1/6の確率でそれぞれ別な部屋に飛ばされる。 そこでロボットAは光を感知した。光を感知できる確率はA, B, C, D　それぞれ 1/10, 1/5, 2/5, 1/10 とする。

(1) 部屋Aにいる確率を求めよ.

(2) 再度「停留」コマンドを選択した後に、光を感知した。部屋Aにいる確率を求めよ。

(1) について解く。

条件は次の通り:

(a) 最初部屋Aにいる、
(b)「停留」コマンドを選択し,確率 1/2で同じ部屋にとどまるが、1/6の確率でそれぞれ別な部屋に飛ばされる。
(c)　光を感知した。光を感知できる確率はA, B, C, D　それぞれ 1/10, 1/5, 2/5, 1/10

ベイズフィルタ

(1) 「停留」コマンドを選択した前の時点での存在確率と状態遷移確率の積」の総和によってそれぞれの状態に存在する「仮の値」を求め、
(2) (1)の値に観測確率を掛けて、存在確率に比例するG値を求め、
(3) G を正規化することで現在の時点での存在確率 F を求める。

In [1]:
import numpy as np
In [2]:
# ベイズフィルタ
# 前提: 存在確率を [A, B, C, D] で表す。(A, B, C, Dは確率、総和は1.0)
# 初期状態(a): Aにいるのだから、
F_0 = np.array([1.0, 0, 0, 0])
# 移動確率を MoveProb で表す
MoveProb = np.array([1/2, 1/6, 1/6, 1/6])
# (1) 「停留」コマンドを選択した前の時点での存在確率と状態遷移確率の積」の総和によってそれぞれの状態に存在する「仮の値」を求める
S1_tmp = F_0[0] * MoveProb
S1_tmp
Out[2]:
array([0.5       , 0.16666667, 0.16666667, 0.16666667])
In [3]:
# 光を観測する確率分布を Light = [1/10, 1/5, 2/5, 1/10] で表す
Light = np.array([1/10, 1/5, 2/5, 1/10] )

# (2)  (1)の値に観測確率を掛けて、存在確率に比例するG値を求める
G1 = S1_tmp * Light
G1
Out[3]:
array([0.05      , 0.03333333, 0.06666667, 0.01666667])
In [4]:
# 見て分かるように、G1は確率ではない。総和が１にならない
sum(G1)
Out[4]:
0.16666666666666669
In [5]:
# (3) G を正規化することで現在の時点での存在確率 F を求める
F_1 = G1/sum(G1)
F_1
Out[5]:
array([0.3, 0.2, 0.4, 0.1])

同じ問題を粒子フィルタで解いてみる

注意1: 乱数を用いるので同じ答えが得られる保証はない
注意2: 粒子の分布が存在確率を表す

粒子フィルタ

(1) 粒子ごとに移動確率に基づいて、次の「仮の」分布状態を求める(サンプリング)
(2) 「仮の」分布状態と、観測確率の積によって重み付けする
(3) 重み付けにより、乱数によって粒子の分布状態を求める(リサンプリング)

In [6]:
import numpy as np

# ここでは粒子の個数を100 とする
Num = 100
# 前提: 粒子の分布を [A, B, C, D] で表す。(A, B, C, Dは粒子の個数、総和は100)
# 初期状態(a): Aにいるのだから、
P_0 = np.array([100, 0, 0, 0])
In [7]:
# (1) 粒子ごとに移動確率に基づいて、次の「仮の」分布状態を求める
# np.random.multinomial(Num, ProbDist) : Num個の粒子を確率分布ProbDistに基づきサンプリングする
# 移動確率を MoveProb で表す
MoveProb = np.array([1/2, 1/6, 1/6, 1/6])
P_1tmp = np.random.multinomial(Num, MoveProb)
P_1tmp
Out[7]:
array([46, 16, 21, 17])
In [8]:
# 光を観測する確率分布を Light = [1/10, 1/5, 2/5, 1/10] で表す
Light = np.array([1/10, 1/5, 2/5, 1/10] )

# (2)「仮の」分布状態と、観測確率の積によって重み付けする
W_1 = Light*P_1tmp
W_1
Out[8]:
array([4.6, 3.2, 8.4, 1.7])
In [9]:
# 見て分かるように、W_1は確率ではなく、粒子の個数でもない。
sum(W_1)
Out[9]:
17.900000000000002
In [10]:
# (3) 重み付けにより、乱数によって粒子の分布状態を求める(リサンプリング)
# 重み付けを「確率分布」に変換して　np.random.multinomial(Num, ProbDist)　を用いてサンプリングする
# そのための変換
PD_1 = W_1/sum(W_1)
PD_1
Out[10]:
array([0.25698324, 0.17877095, 0.46927374, 0.09497207])
In [11]:
# 最終的な粒子の分布
P_1 = np.random.multinomial(Num, PD_1)
P_1
Out[11]:
array([19, 15, 56, 10])

(2) 再度「停留」コマンドを選択した後に、光を感知した。部屋Aにいる確率を求めよ。

ベイズフィルタ

確認: (1)の後の状態確率

In [12]:
F_1
Out[12]:
array([0.3, 0.2, 0.4, 0.1])
In [13]:
# (1) 「停留」コマンドを選択した前の時点での存在確率と状態遷移確率の積」の総和によってそれぞれの状態に存在する「仮の値」を求める
tmp = F_1[0]* MoveProb
tmp
Out[13]:
array([0.15, 0.05, 0.05, 0.05])

注意: 上の値は、前の状態がAである確率に、移動確率をかけたもの。前の状態がB, C, Dそれぞれの可能性も考えないといけない。

In [14]:
# rollメソッドを用いてそれぞれの部屋からの移動確率の計算に用いる: np.roll(List, int) --- Listの中身をintだけ回転(シフト)させる
np.roll(MoveProb,1)
Out[14]:
array([0.16666667, 0.5       , 0.16666667, 0.16666667])
In [15]:
S2_tmp = F_1[0]*MoveProb + F_1[1]*np.roll(MoveProb,1)+F_1[2]*np.roll(MoveProb,2)+F_1[3]*np.roll(MoveProb,3)
S2_tmp
Out[15]:
array([0.26666667, 0.23333333, 0.3       , 0.2       ])
In [16]:
# もっと簡単に書ける
sum([F_1[i] * np.roll(MoveProb,i) for i in range(4)])
Out[16]:
array([0.26666667, 0.23333333, 0.3       , 0.2       ])
In [17]:
# (2)   (1)の値に観測確率を掛けて、存在確率に比例するG値を求める
G2 = S2_tmp * Light
G2
Out[17]:
array([0.02666667, 0.04666667, 0.12      , 0.02      ])
In [18]:
# (3) G を正規化することで現在の時点での存在確率 F を求める
F_2 = G2/sum(G2)
F_2
Out[18]:
array([0.125  , 0.21875, 0.5625 , 0.09375])

粒子フィルタ

確認: (1)の後の状態確率

In [19]:
P_1
Out[19]:
array([19, 15, 56, 10])
In [20]:
# (1) 粒子ごとに移動確率に基づいて、次の「仮の」分布状態を求める

dist_tmp=[np.random.multinomial(P_1[i], MoveProb) for i in range(4)]
dist_tmp
Out[20]:
[array([11,  1,  4,  3]),
 array([9, 2, 3, 1]),
 array([26, 11, 11,  8]),
 array([3, 3, 3, 1])]
In [21]:
# 上はみな０番目の要素が「今いる部屋にとどまる」粒子の個数になっているので、A,B,C,Dそれぞれに分けなければならない
P_2tmp = dist_tmp[0]+np.roll(dist_tmp[1],1)+np.roll(dist_tmp[2],2)+np.roll(dist_tmp[3],3)
P_2tmp
Out[21]:
array([26, 21, 33, 20])
In [22]:
# 上はもっと簡単にかける
sum([np.roll(dist_tmp[i],i) for i in range(4)])
Out[22]:
array([26, 21, 33, 20])
In [23]:
# (2)「仮の」分布状態と、観測確率の積によって重み付けする
W_2 = Light*P_2tmp
W_2
Out[23]:
array([ 2.6,  4.2, 13.2,  2. ])
In [24]:
# (3) 重み付けにより、乱数によって粒子の分布状態を求める(リサンプリング)
# 重み付けを「確率分布」に変換して　np.random.multinomial(Num, ProbDist)　を用いてサンプリングする
# そのための変換
PD_2 = W_2/sum(W_2)
PD_2
Out[24]:
array([0.11818182, 0.19090909, 0.6       , 0.09090909])
In [25]:
# 最終的な粒子の分布
P_2 = np.random.multinomial(Num, PD_2)
P_2
Out[25]:
array([11, 19, 59, 11])