From b5ecb67611f3011260a594a8a1ee544ef0226a82 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Tue, 30 Jan 2018 16:28:10 -0800
Subject: [PATCH] keep implementing

---
 Localization/particle_filter/particle_filter.py | 17 ++++++++++-------
 1 file changed, 10 insertions(+), 7 deletions(-)

diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py
index 729268f3..93357bf5 100644
--- a/Localization/particle_filter/particle_filter.py
+++ b/Localization/particle_filter/particle_filter.py
@@ -6,13 +6,6 @@ author: Atsushi Sakai (@Atsushi_twi)
 
 """
 
-#  MAX_RANGE=20;%最大観測距離
-#  NP=100;%パーティクル数
-#  NTh=NP/2.0;%リサンプリングを実施する有効パーティクル数
-
-#  px=repmat(xEst,1,NP);%パーティクル格納変数
-#  pw=zeros(1,NP)+1/NP;%重み変数
-
 #  tic;
 #  %movcount=0;
 #  % Main loop
@@ -138,6 +131,11 @@ Rsim = np.diag([1.0, math.radians(30.0)])**2
 
 DT = 0.1  # time tick [s]
 SIM_TIME = 50.0  # simulation time [s]
+MAX_RANGE = 20.0  # maximum observation range
+
+# Particle filter parameter
+NP = 100  # Number of Particle
+NTh = NP / 2.0  # Number of particle for re-sampling
 
 show_animation = True
 
@@ -280,6 +278,11 @@ def main():
     xTrue = np.matrix(np.zeros((4, 1)))
     PEst = np.eye(4)
 
+    px = np.matrix(np.zeros((4, NP)))  # Particle store
+    pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP  # Particle weight
+    #  print(px)
+    #  print(pw)
+
     xDR = np.matrix(np.zeros((4, 1)))  # Dead reckoning
 
     # history