-
Notifications
You must be signed in to change notification settings - Fork 58
Expand file tree
/
Copy pathparticle_filter_normal.py
More file actions
98 lines (82 loc) · 2.14 KB
/
particle_filter_normal.py
File metadata and controls
98 lines (82 loc) · 2.14 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
# -*- coding: utf-8 -*-
"""
Particle Filter
(IMU with PF model version)
"""
import numpy
import math
class ParticleFilterNormal:
def __init__(self):
pass
def f(self, dt, x, u):
""" Transition model
- 状態方程式
x_t = f(x_t-1, u) + w
w ~ N(0, sigma)
"""
return x
def likelihood(self, y, x):
""" Likelihood function
- 尤度関数
p(y|x) ~ exp(-1/2 * (|y-h(x)|.t * sigma * |y-h(x)|)
- 観測モデル
z = h(x) + v
v ~ N(0, sigma)
Parameters
----------
y : 観測 Observation
x : 予測 Predicted particle
Returns
-------
likelihood : 尤度 Likelihood
"""
return 1.0
def resampling(self, X, W, M):
""" Resampling
- 等間隔サンプリング
M : パーティクルの数 num of particles
"""
X_resampled = []
Minv = 1/float(M)
r = numpy.random.rand() * Minv
c = W[0]
i = 0
for m in range(M):
U = r + m * Minv
while U > c:
i += 1
c += W[i]
X_resampled.append(X[i])
return X_resampled
def pf_step(self, X, u, y, N):
"""One Step of Sampling Importance Resampling for Particle Filter
Parameters
----------
X : array of [float|array]
状態 List of state set
u : float or array
制御入力 Control input
y : float or array
観測 Observation set
N : int
パーティクルの数 num of particles
Returns
-------
X_resampled : array of [float|array]
次の状態 List updated state
"""
# 初期化
X_predicted = range(N)
weight = range(N)
for i in range(N):
# 推定 prediction
X_predicted[i] = self.f(X[i], u)
# 更新 update
weight[i] = self.likelihood(y, X_predicted[i])
# 正規化 normalization
weight_sum = sum(weight) # 総和 the sum of weights
for i in range(N):
weight[i] /= weight_sum
# リサンプリング re-sampling (if necessary)
X_resampled = self.resampling(X_predicted, weight, N)
return X_resampled