def __init__(self, map_file, num, alpha, log_file_path, r_step, s_search):
print 'Intializing particle filter'
# TODO: load the map first
self.visuvalize = visuvalize.Visuvalize(map_file, r_step, s_search)
self.rotation_step = int(r_step)
# initialize set of particles
self.particles = []
# read logs and based on the data propogate or update
self.d = read_log_data.DataReader(log_file_path)
# number of particles
self.num = num
# alphas for motion model
self.alpha = alpha
# initial odom
self.initial_odom = np.array([0, 0, 0])
# check if the robot has moved to do resmapling
self.isMoved = False
self.start = True
self.laser_count = 0
self.z_test = []
# read distance table
self.z_required = np.load('distance_table.npy', mmap_mode='r')
# generate random particles
self.generate_particles = False
# laser data
self.ll = []
#img count
self.img_count = 0
# for using vectorization
self.particles_weight = np.ones(self.num) * 1.0 / self.num
self.particles_pose = np.ndarray(shape=(self.num, 3), dtype=float)
# initialize the motion model and mesurement model
self.motion_model = motion_model.MotionModel(alpha, [0, 0, 0],
'normal')
# measeurement model
self.measurement_model = measurement_model.MeasurementModel(
self.visuvalize.global_map, self.visuvalize.distance_table,
self.rotation_step)
def __init__(self, aPose, aDt, aScanRng_m, aScanAng_rad, aLandMarks):
""""コンストラクタ
引数：
aPose：姿勢
aPose[0, 0]：x座標[m]
aPose[1, 0]：y座標[m]
aPose[2, 0]：方角(rad)
aDt：演算周期[sec]
aScanRng_m：走査距離[m]
aScanAng_rad：走査角度[rad]
aLandMarks：ランドマーク
[[1番目LMのX座標, 1番目LMのY座標]
[2番目LMのX座標, 2番目LMのY座標]
：
[n番目LMのX座標, n番目LMのY座標]]
"""
self.__mScnSnsr = ScanSensor(aScanRng_m, aScanAng_rad, aLandMarks)
self.__mScnSnsr.setNoiseParam(5, 2, 2)
self.__mMvMdl = mm.MotionModel(aDt, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1)
self.__mTrjEst = TrajectoryEstimator(aPose)
#---------- 制御周期 ----------
self.__mDt = aDt
self.__mTime = 0
#---------- 姿勢 ----------
self.__mPosesActu = [aPose] # 姿勢（実際値）
#---------- 制御 ----------
self.__mCtr = []
#---------- 観測 ----------
self.__mObsActu = []
self.__mObsTrue = []
self.__mHalfEdges = []
# 誤差楕円の信頼区間[%]
self.__mConfidence_interval = 99.0
self.__mEllipse = error_ellipse.ErrorEllipse(
self.__mConfidence_interval)
self.__mScnSnsr.scan(aPose)
self.__observe(aPose, len(self.__mPosesActu) - 1, self.__mTime)
# ガウス・ニュートン法による、反復回数を決定する閾値
# Δxの２乗和(Δx.T・Δx)の値が閾値以下となったら計算終了
self.__DELTA_SUM_TH = 0.01
self.__isCalc = False # 軌跡算出可否判定
self.__loopCnt = 0.0 # 計算反復回数
self.__deltaSum = 0.0 # Δxの２乗和(Δx.T・Δx)
self.__det = 0.0 # 情報行列Hの行列式
self.__cond = 0.0 # 情報行列Hの条件数
def __init__(self, state, coef, w, global_map, table, rotation_step):
# initialize pose
self.pose = state
# weight of the particle
self.weight = w
# initialize the motion model and mesurement model
self.motion_model = motion_model.MotionModel(coef, [0, 0, 0], 'normal')
# measeurement model
self.measurement_model = measurement_model.MeasurementModel(
global_map, table, rotation_step)