class StateCoplanarity:
def __init__(self):
# ----- Set parameters here! ----- #
self.M = 512 # total number of particles パーティクルの数
self.f = 924.1770935 # focus length of camera [px] カメラの焦点距離 [px]
# Kalman Filter
self.noise_x_sys = 0.015 # system noise of position (SD) 位置のシステムノイズ（標準偏差）
self.noise_a_sys = 0.1 # system noise of acceleration (SD) 加速度のシステムノイズ（標準偏差）
self.noise_g_sys = 0.01 # system noise of orientation (SD) 角度のシステムノイズ（標準偏差）
self.noise_a_obs = 0.001 # observation noise of acceleration (SD) 加速度の観測ノイズ（標準偏差）
self.noise_g_obs = 0.0001 # observation noise of orientation (SD) 角度の観測ノイズ（標準偏差）
# Particle Filter
self.PFnoise_coplanarity_obs = 0.01 # observation noise of coplanarity (SD) 共面条件の観測ノイズ（標準偏差）
# ----- Set parameters here! ----- #
self.init()
def init(self):
self.isFirstTimeIMU = True
self.isFirstTimeCamera = True
self.lock = False
self.step = 1
self.t = 0.0
self.t1 = 0.0
self.t_camera = 0.0
self.t1_camera = 0.0
self.accel1 = np.array([0.0, 0.0, 0.0])
self.accel2 = np.array([0.0, 0.0, 0.0])
self.accel3 = np.array([0.0, 0.0, 0.0])
self.initKalmanFilter()
self.initParticleFilter()
def initKalmanFilter(self):
self.mu = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0])
self.mu1 = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0])
self.sigma = np.zeros([12,12])
self.C = np.array([ [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]])
self.Q = np.diag([self.noise_x_sys**2,self.noise_x_sys**2,self.noise_x_sys**2,0.0,0.0,0.0,self.noise_a_sys**2,self.noise_a_sys**2,self.noise_a_sys**2,self.noise_g_sys**2,self.noise_g_sys**2,self.noise_g_sys**2]) # sys noise
self.R = np.diag([self.noise_a_obs**2,self.noise_a_obs**2,self.noise_a_obs**2,self.noise_g_obs**2,self.noise_g_obs**2,self.noise_g_obs**2]) # obs noise
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("Coplanarity")
self.pf.setFocus(self.f)
self.pf.setParameter(self.noise_x_sys, self.PFnoise_coplanarity_obs) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 0
"""
This method is called from Sensor class when new IMU sensor data are arrived.
time_ : time (sec)
accel : acceleration in global coordinates
ori : orientaion
"""
def setSensorData(self, time_, accel, ori):
# Count
self.count+=1
# If process is locked by Image Particle Filter, do nothing
if(self.lock):
print("locked")
return
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
if(self.isFirstTimeIMU):
#init mu
self.mu = np.array([0.0,0.0,0.0,
0.0,0.0,0.0,
accel[0],accel[1],accel[2],
ori[0],ori[1],ori[2]])
else:
#observation
Y = np.array([accel[0],accel[1],accel[2],
ori[0],ori[1],ori[2]])
dt2 = 0.5 * self.dt * self.dt
#dt3 = (1.0 / 6.0) * dt2 * self.dt
A = np.array([[1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0,0.0],
[0.0,1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0],
[0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0],
[0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]])
#Qt = np.diag([dt2,dt2,dt2,self.dt,self.dt,self.dt,1.0,1.0,1.0,self.dt,self.dt,self.dt])
#Q = Qt.dot(self.Q)
"""
self.accel3 = copy.deepcopy(self.accel2)
self.accel2 = copy.deepcopy(self.accel1)
self.accel1 = copy.deepcopy(accel)
if(Util.isDeviceMoving(self.accel1[0]) == False and Util.isDeviceMoving(self.accel2[0]) == False and Util.isDeviceMoving(self.accel3[0]) == False):
self.mu[3] = 0.0
if(Util.isDeviceMoving(self.accel1[1]) == False and Util.isDeviceMoving(self.accel2[1]) == False and Util.isDeviceMoving(self.accel3[1]) == False):
self.mu[4] = 0.0
if(Util.isDeviceMoving(self.accel1[2]) == False and Util.isDeviceMoving(self.accel2[2]) == False and Util.isDeviceMoving(self.accel3[2]) == False):
self.mu[5] = 0.0
"""
self.mu, self.sigma = KF.execKF1Simple(Y,self.mu,self.sigma,A,self.C,self.Q,self.R)
if(self.isFirstTimeIMU):
self.isFirstTimeIMU = False
"""
This method is called from Image class when new camera image data are arrived.
time_ : time (sec)
keypointPairs : list of KeyPointPair class objects
"""
def setImageData(self, time_, keypointPairs):
# If IMU data has not been arrived yet, do nothing
if(self.isFirstTimeIMU):
return
# Count
self.count+=1
########################
print("===================================")
print("step "+str(self.step))
###########################
# If first time, save mu and don't do anything else
if(self.isFirstTimeCamera):
self.isFirstTimeCamera = False
self.mu1 = copy.deepcopy(self.mu) # save mu[t] as mu[t-1]
self.t_camera = time_
self.step += 1
return
# Lock IMU process
self.lock = True
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
self.t1_camera = self.t_camera
self.t_camera = time_
dt_camera = self.t_camera - self.t1_camera
# create particle from state vector
self.X = self.createParticleFromStateVector(self.mu, self.sigma)
# create X1 from mu[t-1]
X1 = Particle()
X1.initWithMu(self.mu1)
self.saveXYZasCSV(self.X,"1") ##############################
# exec particle filter
self.X = self.pf.pf_step(self.X, X1, self.dt, dt_camera, keypointPairs, self.M)
self.saveXYZasCSV(self.X,"2") ##############################
# create state vector from particle set
self.mu, self.sigma = self.createStateVectorFromParticle(self.X)
# save mu[t] as mu[t-1]
self.mu1 = copy.deepcopy(self.mu)
# Step (camera only observation step)
self.step += 1
# Unlock IMU process
self.lock = False
"""
save (X,Y,Z) of particles as CSV file
"""
def saveXYZasCSV(self,X,appendix):
x = []
for X_ in X:
x.append(X_.x)
date = datetime.datetime.now()
#datestr = date.strftime("%Y%m%d_%H%M%S_") + "%04d" % (date.microsecond // 1000)
#np.savetxt('./data/plot3d/'+datestr+'_xyz_'+appendix+'.csv', x, delimiter=',')
datestr = date.strftime("%Y%m%d_%H%M%S_")
np.savetxt('./data/plot3d/'+datestr+str(self.count)+'_xyz_'+appendix+'.csv', x, delimiter=',')
"""
create particle set from state vector
"""
def createParticleFromStateVector(self, mu, sigma):
X = []
for i in range(self.M):
particle = Particle()
particle.initWithStateVector(mu, sigma)
X.append(particle)
return X
"""
create state vector from particle set
"""
def createStateVectorFromParticle(self, X):
x = []
v = []
a = []
o = []
for X_ in X:
x.append(X_.x)
v.append(X_.v)
a.append(X_.a)
o.append(X_.o)
x_mu = np.mean(x, axis=0)
v_mu = np.mean(v, axis=0)
a_mu = np.mean(a, axis=0)
o_mu = np.mean(o, axis=0)
x_var = np.var(x, axis=0)
v_var = np.var(v, axis=0)
a_var = np.var(a, axis=0)
o_var = np.var(o, axis=0)
mu = np.array([x_mu[0],x_mu[1],x_mu[2],v_mu[0],v_mu[1],v_mu[2],a_mu[0],a_mu[1],a_mu[2],o_mu[0],o_mu[1],o_mu[2]])
sigma = np.diag([x_var[0],x_var[1],x_var[2],v_var[0],v_var[1],v_var[2],a_var[0],a_var[1],a_var[2],o_var[0],o_var[1],o_var[2]])
return mu, sigma
"""
This method is called from "Main.py"
return estimated state vector
"""
def getState(self):
x = np.array([self.mu[0],self.mu[1],self.mu[2]])
v = np.array([self.mu[3],self.mu[4],self.mu[5]])
a = np.array([self.mu[6],self.mu[7],self.mu[8]])
o = np.array([self.mu[9],self.mu[10],self.mu[11]])
return x,v,a,o
class StateRBPF:
def __init__(self):
# ----- Set parameters here! ----- #
self.M = 256 # total number of particles パーティクルの数
self.f = 1575.54144 # focus length of camera [px] カメラの焦点距離 [px]
# Particle Filter
self.noise_a_sys = 0.01 # system noise of acceleration 加速度のシステムノイズ
self.noise_g_sys = 0.01 # system noise of gyro ジャイロのシステムノイズ
self.noise_a_sys_camera = 0.1 # system noise of acceleration (at camera step) 加速度のシステムノイズ(カメラ観測時)
self.noise_camera = 0.001 # observation noise of camera カメラの観測ノイズ
# ----- Set parameters here! ----- #
self.init()
def init(self):
self.isFirstTimeIMU = True
self.isFirstTimeCamera = True
self.lock = False
self.t = 0.0
self.t1 = 0.0
self.accel1 = np.array([0.0, 0.0, 0.0])
self.accel2 = np.array([0.0, 0.0, 0.0])
self.accel3 = np.array([0.0, 0.0, 0.0])
self.initParticleFilter()
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("RBPF")
self.pf.setFocus(self.f)
self.pf.setParameter(
self.noise_a_sys, self.noise_g_sys, self.noise_a_sys_camera,
self.noise_camera) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 1
self.step = 1
def initParticle(self, accel, ori):
X = []
for i in xrange(self.M):
particle = Particle()
particle.initWithIMU(accel, ori)
X.append(particle)
return X
def setObservationModel(self, observation_):
self.pf.setObservationModel(observation_)
"""
This method is called from "sensor.py" when new IMU sensor data are arrived.
time : time (sec)
accel : acceleration in global coordinates
ori : orientaion
"""
def setSensorData(self, time_, accel, ori):
# If process is locked by Image Particle Filter, do nothing
if (self.lock):
print("locked")
return
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
if (self.isFirstTimeIMU):
# init particle
self.X = self.initParticle(accel, ori)
else:
# is Device Moving
self.accel3 = copy.deepcopy(self.accel2)
self.accel2 = copy.deepcopy(self.accel1)
self.accel1 = copy.deepcopy(accel)
isMoving = [True, True, True]
if (Util.isDeviceMoving(self.accel1[0]) == False
and Util.isDeviceMoving(self.accel2[0]) == False
and Util.isDeviceMoving(self.accel3[0]) == False):
isMoving[0] = False
if (Util.isDeviceMoving(self.accel1[1]) == False
and Util.isDeviceMoving(self.accel2[1]) == False
and Util.isDeviceMoving(self.accel3[1]) == False):
isMoving[1] = False
if (Util.isDeviceMoving(self.accel1[2]) == False
and Util.isDeviceMoving(self.accel2[2]) == False
and Util.isDeviceMoving(self.accel3[2]) == False):
isMoving[2] = False
# exec particle filter
self.X = self.pf.pf_step_IMU(self.X, self.dt, accel, ori, isMoving,
self.M)
""" The code below is used to get loglikelihood to decide parameters.
self.X, likelihood = self.pf.pf_step_IMU(self.X, self.t - self.t1, accel, ori, self.M)
self.loglikelihood += math.log(likelihood)
self.count += 1
if(self.count==300):
print(str(self.loglikelihood))
"""
if (self.isFirstTimeIMU):
self.isFirstTimeIMU = False
# Count
self.count += 1
"""
This method is called from Image class when new camera image data are arrived.
time_ : time (sec)
keypointPairs : list of KeyPointPair class objects
"""
def setImageData(self, time_, keypoints):
# If IMU data has not been arrived yet, do nothing
if (self.isFirstTimeIMU):
return
if (self.isFirstTimeCamera):
self.isFirstTimeCamera = False
# Lock IMU process
self.lock = True
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
# covariance matrix of position
P = self.createPositionCovarianceMatrixFromParticle(self.X)
#self.saveXYZasCSV(self.X,"1")
# exec particle filter
self.X = self.pf.pf_step_camera(self.X, self.dt, keypoints, self.step,
P, self.M)
#self.saveXYZasCSV(self.X,"2")
# Count
self.count += 1
# Step (camera only observation step)
self.step += 1
# Unlock IMU process
self.lock = False
"""
print (X,Y,Z) of particles
"""
def printXYZ(self, X):
print("-----")
for x in X:
x.printXYZ()
"""
save (X,Y,Z) of particles as CSV file
"""
def saveXYZasCSV(self, X, appendix):
x = []
for X_ in X:
x.append(X_.x)
date = datetime.datetime.now()
datestr = date.strftime("%Y%m%d_%H%M%S_")
np.savetxt('./data/plot3d/' + datestr + 'xyz_' + appendix + '.csv',
x,
delimiter=',')
"""
create covariance matrix of position from particle set
"""
def createPositionCovarianceMatrixFromParticle(self, X):
x = []
for X_ in X:
if (len(x) == 0):
x = X_.x
else:
x = np.vstack((x, X_.x))
P = np.cov(x.T)
return P
"""
This method is called from "Main.py"
return estimated state vector
"""
def getState(self):
x = []
v = []
a = []
o = []
for X_ in self.X:
x.append(X_.x)
v.append(X_.v)
a.append(X_.a)
o.append(X_.o)
#print(np.var(x, axis=0))
return np.mean(x, axis=0), np.mean(v, axis=0), np.mean(
a, axis=0), np.mean(o, axis=0)
class StateCoplanarity:
def __init__(self):
# ----- Set parameters here! ----- #
self.M = 512 # total number of particles パーティクルの数
self.f = 924.1770935 # focus length of camera [px] カメラの焦点距離 [px]
# Kalman Filter
self.noise_x_sys = 0.015 # system noise of position (SD) 位置のシステムノイズ（標準偏差）
self.noise_a_sys = 0.1 # system noise of acceleration (SD) 加速度のシステムノイズ（標準偏差）
self.noise_g_sys = 0.01 # system noise of orientation (SD) 角度のシステムノイズ（標準偏差）
self.noise_a_obs = 0.001 # observation noise of acceleration (SD) 加速度の観測ノイズ（標準偏差）
self.noise_g_obs = 0.0001 # observation noise of orientation (SD) 角度の観測ノイズ（標準偏差）
# Particle Filter
self.PFnoise_coplanarity_obs = 0.01 # observation noise of coplanarity (SD) 共面条件の観測ノイズ（標準偏差）
# ----- Set parameters here! ----- #
self.init()
def init(self):
self.isFirstTimeIMU = True
self.isFirstTimeCamera = True
self.lock = False
self.step = 1
self.t = 0.0
self.t1 = 0.0
self.t_camera = 0.0
self.t1_camera = 0.0
self.accel1 = np.array([0.0, 0.0, 0.0])
self.accel2 = np.array([0.0, 0.0, 0.0])
self.accel3 = np.array([0.0, 0.0, 0.0])
self.initKalmanFilter()
self.initParticleFilter()
def initKalmanFilter(self):
self.mu = np.array(
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.mu1 = np.array(
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.sigma = np.zeros([12, 12])
self.C = np.array(
[[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])
self.Q = np.diag([
self.noise_x_sys**2, self.noise_x_sys**2, self.noise_x_sys**2, 0.0,
0.0, 0.0, self.noise_a_sys**2, self.noise_a_sys**2,
self.noise_a_sys**2, self.noise_g_sys**2, self.noise_g_sys**2,
self.noise_g_sys**2
]) # sys noise
self.R = np.diag([
self.noise_a_obs**2, self.noise_a_obs**2, self.noise_a_obs**2,
self.noise_g_obs**2, self.noise_g_obs**2, self.noise_g_obs**2
]) # obs noise
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("Coplanarity")
self.pf.setFocus(self.f)
self.pf.setParameter(self.noise_x_sys, self.PFnoise_coplanarity_obs
) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 0
"""
This method is called from Sensor class when new IMU sensor data are arrived.
time_ : time (sec)
accel : acceleration in global coordinates
ori : orientaion
"""
def setSensorData(self, time_, accel, ori):
# Count
self.count += 1
# If process is locked by Image Particle Filter, do nothing
if (self.lock):
print("locked")
return
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
if (self.isFirstTimeIMU):
#init mu
self.mu = np.array([
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, accel[0], accel[1], accel[2],
ori[0], ori[1], ori[2]
])
else:
#observation
Y = np.array(
[accel[0], accel[1], accel[2], ori[0], ori[1], ori[2]])
dt2 = 0.5 * self.dt * self.dt
#dt3 = (1.0 / 6.0) * dt2 * self.dt
A = np.array(
[[
1.0, 0.0, 0.0, self.dt, 0.0, 0.0, dt2, 0.0, 0.0, 0.0, 0.0,
0.0
],
[
0.0, 1.0, 0.0, 0.0, self.dt, 0.0, 0.0, dt2, 0.0, 0.0, 0.0,
0.0
],
[
0.0, 0.0, 1.0, 0.0, 0.0, self.dt, 0.0, 0.0, dt2, 0.0, 0.0,
0.0
],
[
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, self.dt,
0.0, 0.0, 0.0, 0.0, 0.0
],
[
0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, self.dt,
0.0, 0.0, 0.0, 0.0
],
[
0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, self.dt,
0.0, 0.0, 0.0
],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])
#Qt = np.diag([dt2,dt2,dt2,self.dt,self.dt,self.dt,1.0,1.0,1.0,self.dt,self.dt,self.dt])
#Q = Qt.dot(self.Q)
"""
self.accel3 = copy.deepcopy(self.accel2)
self.accel2 = copy.deepcopy(self.accel1)
self.accel1 = copy.deepcopy(accel)
if(Util.isDeviceMoving(self.accel1[0]) == False and Util.isDeviceMoving(self.accel2[0]) == False and Util.isDeviceMoving(self.accel3[0]) == False):
self.mu[3] = 0.0
if(Util.isDeviceMoving(self.accel1[1]) == False and Util.isDeviceMoving(self.accel2[1]) == False and Util.isDeviceMoving(self.accel3[1]) == False):
self.mu[4] = 0.0
if(Util.isDeviceMoving(self.accel1[2]) == False and Util.isDeviceMoving(self.accel2[2]) == False and Util.isDeviceMoving(self.accel3[2]) == False):
self.mu[5] = 0.0
"""
self.mu, self.sigma = KF.execKF1Simple(Y, self.mu, self.sigma, A,
self.C, self.Q, self.R)
if (self.isFirstTimeIMU):
self.isFirstTimeIMU = False
"""
This method is called from Image class when new camera image data are arrived.
time_ : time (sec)
keypointPairs : list of KeyPointPair class objects
"""
def setImageData(self, time_, keypointPairs):
# If IMU data has not been arrived yet, do nothing
if (self.isFirstTimeIMU):
return
# Count
self.count += 1
########################
print("===================================")
print("step " + str(self.step))
###########################
# If first time, save mu and don't do anything else
if (self.isFirstTimeCamera):
self.isFirstTimeCamera = False
self.mu1 = copy.deepcopy(self.mu) # save mu[t] as mu[t-1]
self.t_camera = time_
self.step += 1
return
# Lock IMU process
self.lock = True
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
self.t1_camera = self.t_camera
self.t_camera = time_
dt_camera = self.t_camera - self.t1_camera
# create particle from state vector
self.X = self.createParticleFromStateVector(self.mu, self.sigma)
# create X1 from mu[t-1]
X1 = Particle()
X1.initWithMu(self.mu1)
self.saveXYZasCSV(self.X, "1") ##############################
# exec particle filter
self.X = self.pf.pf_step(self.X, X1, self.dt, dt_camera, keypointPairs,
self.M)
self.saveXYZasCSV(self.X, "2") ##############################
# create state vector from particle set
self.mu, self.sigma = self.createStateVectorFromParticle(self.X)
# save mu[t] as mu[t-1]
self.mu1 = copy.deepcopy(self.mu)
# Step (camera only observation step)
self.step += 1
# Unlock IMU process
self.lock = False
"""
save (X,Y,Z) of particles as CSV file
"""
def saveXYZasCSV(self, X, appendix):
x = []
for X_ in X:
x.append(X_.x)
date = datetime.datetime.now()
#datestr = date.strftime("%Y%m%d_%H%M%S_") + "%04d" % (date.microsecond // 1000)
#np.savetxt('./data/plot3d/'+datestr+'_xyz_'+appendix+'.csv', x, delimiter=',')
datestr = date.strftime("%Y%m%d_%H%M%S_")
np.savetxt('./data/plot3d/' + datestr + str(self.count) + '_xyz_' +
appendix + '.csv',
x,
delimiter=',')
"""
create particle set from state vector
"""
def createParticleFromStateVector(self, mu, sigma):
X = []
for i in range(self.M):
particle = Particle()
particle.initWithStateVector(mu, sigma)
X.append(particle)
return X
"""
create state vector from particle set
"""
def createStateVectorFromParticle(self, X):
x = []
v = []
a = []
o = []
for X_ in X:
x.append(X_.x)
v.append(X_.v)
a.append(X_.a)
o.append(X_.o)
x_mu = np.mean(x, axis=0)
v_mu = np.mean(v, axis=0)
a_mu = np.mean(a, axis=0)
o_mu = np.mean(o, axis=0)
x_var = np.var(x, axis=0)
v_var = np.var(v, axis=0)
a_var = np.var(a, axis=0)
o_var = np.var(o, axis=0)
mu = np.array([
x_mu[0], x_mu[1], x_mu[2], v_mu[0], v_mu[1], v_mu[2], a_mu[0],
a_mu[1], a_mu[2], o_mu[0], o_mu[1], o_mu[2]
])
sigma = np.diag([
x_var[0], x_var[1], x_var[2], v_var[0], v_var[1], v_var[2],
a_var[0], a_var[1], a_var[2], o_var[0], o_var[1], o_var[2]
])
return mu, sigma
"""
This method is called from "Main.py"
return estimated state vector
"""
def getState(self):
x = np.array([self.mu[0], self.mu[1], self.mu[2]])
v = np.array([self.mu[3], self.mu[4], self.mu[5]])
a = np.array([self.mu[6], self.mu[7], self.mu[8]])
o = np.array([self.mu[9], self.mu[10], self.mu[11]])
return x, v, a, o
class StateRBPF:
def __init__(self):
# ----- Set parameters here! ----- #
self.M = 200 # total number of particles パーティクルの数
self.f = 924.1770935 # focus length of camera [px] カメラの焦点距離 [px]
#self.f = 1575.54144 # focus length of camera [px] カメラの焦点距離 [px]
# Particle Filter
self.noise_x_sys = 0.001 # system noise of position (SD) 位置のシステムノイズ（標準偏差）
self.noise_x_sys_coefficient = 0.05 # system noise of position (coefficient) 位置のシステムノイズ（係数）
self.noise_a_sys = 0.1 # system noise of acceleration (SD) 加速度のシステムノイズ（標準偏差）
self.noise_g_sys = 0.01 # system noise of orientation (SD) 角度のシステムノイズ（標準偏差）
self.noise_a_obs = 0.001 # observation noise of acceleration (SD) 加速度の観測ノイズ（標準偏差）
self.noise_g_obs = 0.0001 # observation noise of orientation (SD) 角度の観測ノイズ（標準偏差）
self.noise_camera = 5.0 # observation noise of camera (SD) カメラの観測ノイズ（標準偏差）
self.noise_coplanarity = 0.1 # observation noise of coplanarity (SD) 共面条件の観測ノイズ（標準偏差）
self.init()
def init(self):
self.isFirstTimeIMU = True
self.isFirstTimeCamera = True
self.lock = False
self.t = 0.0
self.t1 = 0.0
self.t_camera = 0.0
self.t1_camera = 0.0
self.accel1 = np.array([0.0, 0.0, 0.0])
self.accel2 = np.array([0.0, 0.0, 0.0])
self.accel3 = np.array([0.0, 0.0, 0.0])
self.gyro = np.array([0.0, 0.0, 0.0])
self.P1 = np.identity(3)
self.initParticleFilter()
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("RBPF")
self.pf.setFocus(self.f)
self.pf.setParameter(self.noise_x_sys, self.noise_a_sys,
self.noise_g_sys, self.noise_camera,
self.noise_coplanarity,
self.noise_x_sys_coefficient
) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 1
self.step = 1
def initParticle(self, accel, ori):
X = []
for i in xrange(self.M):
particle = Particle()
particle.initWithIMU(accel, ori)
X.append(particle)
return X
def setObservationModel(self, observation_):
self.pf.setObservationModel(observation_)
"""
This method is called from "sensor.py" when new IMU sensor data are arrived.
time : time (sec)
accel : acceleration in global coordinates
ori : orientaion
"""
def setSensorData(self, time_, accel, ori, gyro_):
# If process is locked by Image Particle Filter, do nothing
if (self.lock):
print("locked")
return
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
self.gyro = gyro_
if (self.isFirstTimeIMU):
# init particle
self.X = self.initParticle(accel, ori)
else:
# exec particle filter
self.X = self.pf.pf_step_IMU(self.X, self.dt, accel, ori, self.M,
self.isFirstTimeCamera)
if (self.isFirstTimeIMU):
self.isFirstTimeIMU = False
# Count
self.count += 1
"""
This method is called from Image class when new camera image data are arrived.
time_ : time (sec)
keypointPairs : list of KeyPointPair class objects
"""
def setImageData(self, time_, keypoints):
# If IMU data has not been arrived yet, do nothing
if (self.isFirstTimeIMU):
return
########################
#print("=================")
#print("step "+str(self.step)+" count "+str(self.count))
###########################
if (keypoints == "nomatch"):
print("nomatch ***********************")
self.reduce_particle_variance(self.X)
self.count += 1
self.step += 1
return
# Lock IMU process
self.lock = True
#start_time = time.clock()
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
self.t1_camera = self.t_camera
self.t_camera = time_
dt_camera = self.t_camera - self.t1_camera
# covariance matrix of position
P = self.createPositionCovarianceMatrixFromParticle(self.X)
#P *= 0.01
if (self.step > 0 and self.step < 10):
#self.saveXYZasCSV(self.X,"1")
pass
if (self.isFirstTimeCamera):
# exec particle filter
self.X = self.pf.pf_step_camera_firsttime(self.X, self.dt,
keypoints, self.step, P,
self.M)
else:
# exec particle filter
self.X = self.pf.pf_step_camera(self.X, self.dt, keypoints,
self.step, P, self.M, self.X1,
self.P1, dt_camera, self.gyro)
if (self.step > 0 and self.step < 10):
#self.saveXYZasCSV(self.X,"2")
pass
# Get prev position and orientation
prevXx, prevXo = self.getPositionAndOrientation()
self.X1 = Particle()
self.X1.initWithPositionAndOrientation(prevXx, prevXo)
self.P1 = P
# Count
self.count += 1
# Step (camera only observation step)
self.step += 1
#end_time = time.clock()
#print "%f" %(end_time-start_time)
# Unlock IMU process
self.lock = False
if (self.isFirstTimeCamera):
self.isFirstTimeCamera = False
def reduce_particle_variance(self, X):
"""
This method is called when No-resampling = True.
Reduce particle variance to avoid divergence of particles.
"""
x = []
# Calc average of position
for X_ in X:
x.append(X_.x)
average = np.mean(x, axis=0)
# Reduce variance of position
for X_ in X:
difference = X_.x - average
X_.x = average + difference * 0.1
return X
"""
print Landmark (X,Y,Z)
"""
def printLandmark(self, X):
print("-----")
landmarks = self.getLandmarkXYZ(X)
for key, value in landmarks.iteritems():
print(str(key) + " "),
print(value)
"""
return Landmark (X,Y,Z)
"""
def getLandmarkXYZ(self, X):
allLandmarks = {}
# calc sum of landmark XYZ
for x in X:
for landmarkId, landmark in x.landmarks.iteritems():
xyz = landmark.getXYZ()
if (allLandmarks.has_key(landmarkId) == False):
allLandmarks[landmarkId] = xyz
else:
allLandmarks[landmarkId] += xyz
# calc average of landamrk XYZ
for key, value in allLandmarks.iteritems():
value /= float(self.M)
return allLandmarks
"""
print (X,Y,Z) of particles
"""
def printXYZ(self, X):
print("-----")
for x in X:
x.printXYZ()
"""
save (X,Y,Z) of particles as CSV file
"""
def saveXYZasCSV(self, X, appendix):
x = []
for X_ in X:
x.append(X_.x)
date = datetime.datetime.now()
#datestr = date.strftime("%Y%m%d_%H%M%S_") + "%04d" % (date.microsecond // 1000)
#np.savetxt('./data/plot3d/'+datestr+'_xyz_'+appendix+'.csv', x, delimiter=',')
datestr = date.strftime("%Y%m%d_%H%M%S_")
np.savetxt('./data/output/particle_' + datestr + str(self.count) +
'_' + appendix + '.csv',
x,
delimiter=',')
"""
create covariance matrix of position from particle set
"""
def createPositionCovarianceMatrixFromParticle(self, X):
x = []
for X_ in X:
if (len(x) == 0):
x = X_.x
else:
x = np.vstack((x, X_.x))
P = np.cov(x.T)
return P
"""
return estimated state vector of position and orientation
"""
def getPositionAndOrientation(self):
x = []
o = []
for X_ in self.X:
x.append(X_.x)
o.append(X_.o)
return np.mean(x, axis=0), np.mean(o, axis=0)
"""
This method is called from "Main.py"
return estimated state vector
"""
def getState(self):
x = []
v = []
a = []
o = []
for X_ in self.X:
x.append(X_.x)
v.append(X_.v)
a.append(X_.a)
o.append(X_.o)
#print(np.var(x, axis=0))
return np.mean(x, axis=0), np.mean(v, axis=0), np.mean(
a, axis=0), np.mean(o, axis=0)
class StateRBPF:
def __init__(self):
# ----- Set parameters here! ----- #
self.M = 200 # total number of particles パーティクルの数
self.f = 924.1770935 # focus length of camera [px] カメラの焦点距離 [px]
#self.f = 1575.54144 # focus length of camera [px] カメラの焦点距離 [px]
# Particle Filter
self.noise_x_sys = 0.001 # system noise of position (SD) 位置のシステムノイズ（標準偏差）
self.noise_x_sys_coefficient = 0.05 # system noise of position (coefficient) 位置のシステムノイズ（係数）
self.noise_a_sys = 0.1 # system noise of acceleration (SD) 加速度のシステムノイズ（標準偏差）
self.noise_g_sys = 0.01 # system noise of orientation (SD) 角度のシステムノイズ（標準偏差）
self.noise_a_obs = 0.001 # observation noise of acceleration (SD) 加速度の観測ノイズ（標準偏差）
self.noise_g_obs = 0.0001 # observation noise of orientation (SD) 角度の観測ノイズ（標準偏差）
self.noise_camera = 5.0 # observation noise of camera (SD) カメラの観測ノイズ（標準偏差）
self.noise_coplanarity = 0.1 # observation noise of coplanarity (SD) 共面条件の観測ノイズ（標準偏差）
self.init()
def init(self):
self.isFirstTimeIMU = True
self.isFirstTimeCamera = True
self.lock = False
self.t = 0.0
self.t1 = 0.0
self.t_camera = 0.0
self.t1_camera = 0.0
self.accel1 = np.array([0.0, 0.0, 0.0])
self.accel2 = np.array([0.0, 0.0, 0.0])
self.accel3 = np.array([0.0, 0.0, 0.0])
self.gyro = np.array([0.0, 0.0, 0.0])
self.P1 = np.identity(3)
self.initParticleFilter()
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("RBPF")
self.pf.setFocus(self.f)
self.pf.setParameter(self.noise_x_sys, self.noise_a_sys, self.noise_g_sys, self.noise_camera, self.noise_coplanarity, self.noise_x_sys_coefficient) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 1
self.step = 1
def initParticle(self, accel, ori):
X = []
for i in xrange(self.M):
particle = Particle()
particle.initWithIMU(accel, ori)
X.append(particle)
return X
def setObservationModel(self, observation_):
self.pf.setObservationModel(observation_)
"""
This method is called from "sensor.py" when new IMU sensor data are arrived.
time : time (sec)
accel : acceleration in global coordinates
ori : orientaion
"""
def setSensorData(self, time_, accel, ori, gyro_):
# If process is locked by Image Particle Filter, do nothing
if(self.lock):
print("locked")
return
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
self.gyro = gyro_
if(self.isFirstTimeIMU):
# init particle
self.X = self.initParticle(accel, ori)
else:
# exec particle filter
self.X = self.pf.pf_step_IMU(self.X, self.dt, accel, ori, self.M, self.isFirstTimeCamera)
if(self.isFirstTimeIMU):
self.isFirstTimeIMU = False
# Count
self.count+=1
"""
This method is called from Image class when new camera image data are arrived.
time_ : time (sec)
keypointPairs : list of KeyPointPair class objects
"""
def setImageData(self, time_, keypoints):
# If IMU data has not been arrived yet, do nothing
if(self.isFirstTimeIMU):
return
########################
#print("=================")
#print("step "+str(self.step)+" count "+str(self.count))
###########################
if(keypoints == "nomatch"):
print("nomatch ***********************")
self.reduce_particle_variance(self.X)
self.count += 1
self.step += 1
return
# Lock IMU process
self.lock = True
#start_time = time.clock()
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
self.t1_camera = self.t_camera
self.t_camera = time_
dt_camera = self.t_camera - self.t1_camera
# covariance matrix of position
P = self.createPositionCovarianceMatrixFromParticle(self.X)
#P *= 0.01
if(self.step > 0 and self.step < 10):
#self.saveXYZasCSV(self.X,"1")
pass
if(self.isFirstTimeCamera):
# exec particle filter
self.X = self.pf.pf_step_camera_firsttime(self.X, self.dt, keypoints, self.step, P, self.M)
else:
# exec particle filter
self.X = self.pf.pf_step_camera(self.X, self.dt, keypoints, self.step, P, self.M, self.X1, self.P1, dt_camera, self.gyro)
if(self.step > 0 and self.step < 10):
#self.saveXYZasCSV(self.X,"2")
pass
# Get prev position and orientation
prevXx, prevXo = self.getPositionAndOrientation()
self.X1 = Particle()
self.X1.initWithPositionAndOrientation(prevXx, prevXo)
self.P1 = P
# Count
self.count += 1
# Step (camera only observation step)
self.step += 1
#end_time = time.clock()
#print "%f" %(end_time-start_time)
# Unlock IMU process
self.lock = False
if(self.isFirstTimeCamera):
self.isFirstTimeCamera = False
def reduce_particle_variance(self, X):
"""
This method is called when No-resampling = True.
Reduce particle variance to avoid divergence of particles.
"""
x = []
# Calc average of position
for X_ in X:
x.append(X_.x)
average = np.mean(x, axis=0)
# Reduce variance of position
for X_ in X:
difference = X_.x - average
X_.x = average + difference * 0.1
return X
"""
print Landmark (X,Y,Z)
"""
def printLandmark(self,X):
print("-----")
landmarks = self.getLandmarkXYZ(X)
for key, value in landmarks.iteritems():
print(str(key)+" "),
print(value)
"""
return Landmark (X,Y,Z)
"""
def getLandmarkXYZ(self,X):
allLandmarks = {}
# calc sum of landmark XYZ
for x in X:
for landmarkId, landmark in x.landmarks.iteritems():
xyz = landmark.getXYZ()
if(allLandmarks.has_key(landmarkId) == False):
allLandmarks[landmarkId] = xyz
else:
allLandmarks[landmarkId] += xyz
# calc average of landamrk XYZ
for key, value in allLandmarks.iteritems():
value /= float(self.M)
return allLandmarks
"""
print (X,Y,Z) of particles
"""
def printXYZ(self,X):
print("-----")
for x in X:
x.printXYZ()
"""
save (X,Y,Z) of particles as CSV file
"""
def saveXYZasCSV(self,X,appendix):
x = []
for X_ in X:
x.append(X_.x)
date = datetime.datetime.now()
#datestr = date.strftime("%Y%m%d_%H%M%S_") + "%04d" % (date.microsecond // 1000)
#np.savetxt('./data/plot3d/'+datestr+'_xyz_'+appendix+'.csv', x, delimiter=',')
datestr = date.strftime("%Y%m%d_%H%M%S_")
np.savetxt('./data/output/particle_'+datestr+str(self.count)+'_'+appendix+'.csv', x, delimiter=',')
"""
create covariance matrix of position from particle set
"""
def createPositionCovarianceMatrixFromParticle(self, X):
x = []
for X_ in X:
if(len(x)==0):
x = X_.x
else:
x = np.vstack((x,X_.x))
P = np.cov(x.T)
return P
"""
return estimated state vector of position and orientation
"""
def getPositionAndOrientation(self):
x = []
o = []
for X_ in self.X:
x.append(X_.x)
o.append(X_.o)
return np.mean(x, axis=0),np.mean(o, axis=0)
"""
This method is called from "Main.py"
return estimated state vector
"""
def getState(self):
x = []
v = []
a = []
o = []
for X_ in self.X:
x.append(X_.x)
v.append(X_.v)
a.append(X_.a)
o.append(X_.o)
#print(np.var(x, axis=0))
return np.mean(x, axis=0),np.mean(v, axis=0),np.mean(a, axis=0),np.mean(o, axis=0)
class StateCoplanarity:
def __init__(self):
# ----- Set parameters here! ----- #
self.M = 100 # total number of particles パーティクルの数
self.f = 1575.54144 # focus length of camera [px] カメラの焦点距離 [px]
# Kalman Filter
self.noise_a_sys = 0.01 # system noise of acceleration 加速度のシステムノイズ
self.noise_g_sys = 0.01 # system noise of gyro ジャイロのシステムノイズ
self.noise_a_obs = 0.00000001 # observation noise of acceleration 加速度の観測ノイズ
self.noise_g_obs = 0.00000001 # observation noise of gyro ジャイロの観測ノイズ
# Particle Filter
self.PFnoise_a_sys = 10.0 # system noise of acceleration 加速度のシステムノイズ
self.PFnoise_g_sys = 10.0 # system noise of gyro ジャイロのシステムノイズ
self.PFnoise_a_obs = 0.00000001 # observation noise of acceleration 加速度の観測ノイズ
self.PFnoise_g_obs = 0.00000001 # observation noise of gyro ジャイロの観測ノイズ
self.PFnoise_coplanarity_obs = 1.0 # observation noise of coplanarity 共面条件の観測ノイズ
# ----- Set parameters here! ----- #
self.init()
def init(self):
self.isFirstTimeIMU = True
self.isFirstTimeCamera = True
self.lock = False
self.t = 0.0
self.t1 = 0.0
self.accel1 = np.array([0.0, 0.0, 0.0])
self.accel2 = np.array([0.0, 0.0, 0.0])
self.accel3 = np.array([0.0, 0.0, 0.0])
self.initKalmanFilter()
self.initParticleFilter()
def initKalmanFilter(self):
self.mu = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0])
self.mu1 = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0])
self.sigma = np.zeros([12,12])
self.C = np.array([ [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]])
self.Q = np.diag([self.noise_a_sys,self.noise_a_sys,self.noise_a_sys,self.noise_a_sys,self.noise_a_sys,self.noise_a_sys,self.noise_a_sys,self.noise_a_sys,self.noise_a_sys,self.noise_g_sys,self.noise_g_sys,self.noise_g_sys]) # sys noise
self.R = np.diag([self.noise_a_obs,self.noise_a_obs,self.noise_a_obs,self.noise_g_obs,self.noise_g_obs,self.noise_g_obs]) # obs noise
def initParticleFilter(self):
self.pf = ParticleFilter().getParticleFilterClass("Coplanarity")
self.pf.setFocus(self.f)
self.pf.setParameter(self.PFnoise_a_sys, self.PFnoise_g_sys, self.PFnoise_a_obs, self.PFnoise_g_obs, self.PFnoise_coplanarity_obs) #パーティクルフィルタのパラメータ（ノイズ） parameters (noise)
self.X = [] # パーティクルセット set of particles
self.loglikelihood = 0.0
self.count = 0
"""
This method is called from Sensor class when new IMU sensor data are arrived.
time_ : time (sec)
accel : acceleration in global coordinates
ori : orientaion
"""
def setSensorData(self, time_, accel, ori):
# Count
self.count+=1
# If process is locked by Image Particle Filter, do nothing
if(self.lock):
print("locked")
return
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
if(self.isFirstTimeIMU):
#init mu
self.mu = np.array([0.0,0.0,0.0,
0.0,0.0,0.0,
accel[0],accel[1],accel[2],
ori[0],ori[1],ori[2]])
else:
#observation
Y = np.array([accel[0],accel[1],accel[2],
ori[0],ori[1],ori[2]])
dt2 = 0.5 * self.dt * self.dt
#dt3 = (1.0 / 6.0) * dt2 * self.dt
A = np.array([[1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0,0.0],
[0.0,1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0,0.0],
[0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,dt2,0.0,0.0,0.0],
[0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,self.dt,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0]])
Qt = np.diag([dt2,dt2,dt2,self.dt,self.dt,self.dt,1.0,1.0,1.0,self.dt,self.dt,self.dt])
Q = Qt.dot(self.Q)
self.accel3 = copy.deepcopy(self.accel2)
self.accel2 = copy.deepcopy(self.accel1)
self.accel1 = copy.deepcopy(accel)
if(Util.isDeviceMoving(self.accel1[0]) == False and Util.isDeviceMoving(self.accel2[0]) == False and Util.isDeviceMoving(self.accel3[0]) == False):
self.mu[3] = 0.0
if(Util.isDeviceMoving(self.accel1[1]) == False and Util.isDeviceMoving(self.accel2[1]) == False and Util.isDeviceMoving(self.accel3[1]) == False):
self.mu[4] = 0.0
if(Util.isDeviceMoving(self.accel1[2]) == False and Util.isDeviceMoving(self.accel2[2]) == False and Util.isDeviceMoving(self.accel3[2]) == False):
self.mu[5] = 0.0
self.mu, self.sigma = KF.execKF1Simple(Y,self.mu,self.sigma,A,self.C,Q,self.R)
if(self.isFirstTimeIMU):
self.isFirstTimeIMU = False
"""
This method is called from Image class when new camera image data are arrived.
time_ : time (sec)
keypointPairs : list of KeyPointPair class objects
"""
def setImageData(self, time_, keypointPairs):
# Count
self.count+=1
# If IMU data has not been arrived yet, do nothing
if(self.isFirstTimeIMU):
return
# If first time, save mu and don't do anything else
if(self.isFirstTimeCamera):
self.isFirstTimeCamera = False
self.mu1 = copy.deepcopy(self.mu) # save mu[t] as mu[t-1]
return
# Lock IMU process
self.lock = True
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
# create particle from state vector
self.X = self.createParticleFromStateVector(self.mu, self.sigma)
# create X1 from mu[t-1]
X1 = Particle()
X1.initWithMu(self.mu1)
# exec particle filter
##########print(self.count),
self.X = self.pf.pf_step(self.X, X1, self.dt, keypointPairs, self.M)
# create state vector from particle set
self.mu, self.sigma = self.createStateVectorFromParticle(self.X)
# save mu[t] as mu[t-1]
self.mu1 = copy.deepcopy(self.mu)
# Unlock IMU process
self.lock = False
"""
create particle set from state vector
"""
def createParticleFromStateVector(self, mu, sigma):
X = []
for i in range(self.M):
particle = Particle()
particle.initWithStateVector(mu, sigma)
X.append(particle)
return X
"""
create state vector from particle set
"""
def createStateVectorFromParticle(self, X):
x = []
v = []
a = []
o = []
for X_ in X:
x.append(X_.x)
v.append(X_.v)
a.append(X_.a)
o.append(X_.o)
x_mu = np.mean(x, axis=0)
v_mu = np.mean(v, axis=0)
a_mu = np.mean(a, axis=0)
o_mu = np.mean(o, axis=0)
x_var = np.var(x, axis=0)
v_var = np.var(v, axis=0)
a_var = np.var(a, axis=0)
o_var = np.var(o, axis=0)
mu = np.array([x_mu[0],x_mu[1],x_mu[2],v_mu[0],v_mu[1],v_mu[2],a_mu[0],a_mu[1],a_mu[2],o_mu[0],o_mu[1],o_mu[2]])
sigma = np.diag([x_var[0],x_var[1],x_var[2],v_var[0],v_var[1],v_var[2],a_var[0],a_var[1],a_var[2],o_var[0],o_var[1],o_var[2]])
return mu, sigma
"""
This method is called from "Main.py"
return estimated state vector
"""
def getState(self):
x = np.array([self.mu[0],self.mu[1],self.mu[2]])
v = np.array([self.mu[3],self.mu[4],self.mu[5]])
a = np.array([self.mu[6],self.mu[7],self.mu[8]])
o = np.array([self.mu[9],self.mu[10],self.mu[11]])
return x,v,a,o
