def eval_logp_x0(self, particles, t):
"""
Evaluate log p(x_0)
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- t (float): time stamp
"""
N = len(particles)
res = numpy.empty(N)
# Assumes Px0 is either full rang or zero
if ((self.Px0 == 0.0).all()):
x0 = self.x0.ravel()
for i in range(N):
if (numpy.array_equiv(particles[i], x0)):
res[i] = 0.0
else:
res[i] = -numpy.Inf
else:
Pchol = scipy.linalg.cho_factor(self.Px0, check_finite=False)
for i in range(N):
res[i] = kalman.lognormpdf_cho(
particles[i].ravel() - self.x0.ravel(), Pchol)
return res
def logp_xnext(self, particles, next_part, u, t):
"""
Return the log-pdf value for the possible future state 'next'
given input u
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- next_part (array-like): particle estimate for t+1
- u (array-like): input signal
- t (float): time stamps
Returns:
(array-like) with first dimension = N, logp(x_{t+1}|x_t^i)
"""
f = self.calc_f(particles, u, t)
if (f is None):
f = self.f
diff = next_part - f
Q = self.calc_Q(particles, u, t)
if (Q is None):
if (self.Qcholtri.shape[0] == 1):
lpx = kalman.lognormpdf_scalar(diff, self.Qcholtri)
else:
lpx = kalman.lognormpdf_cho_vec(diff, self.Qchol)
else:
N = len(particles)
lpx = numpy.empty(N)
for i in range(N):
Qchol = scipy.linalg.cho_factor(Q[i], check_finite=False)
lpx[i] = kalman.lognormpdf_cho(diff[i], Qchol)
return lpx
def eval_logp_x0(self, particles, t):
"""
Evaluate log p(x_0)
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- t (float): time stamp
"""
N = len(particles)
res = numpy.empty(N)
# Assumes Px0 is either full rang or zero
if ((self.Px0 == 0.0).all()):
x0 = self.x0.ravel()
for i in xrange(N):
if (numpy.array_equiv(particles[i], x0)):
res[i] = 0.0
else:
res[i] = -numpy.Inf
else:
Pchol = scipy.linalg.cho_factor(self.Px0, check_finite=False)
for i in xrange(N):
res[i] = kalman.lognormpdf_cho(particles[i].ravel() - self.x0.ravel(), Pchol)
return res
def logp_xnext(self, particles, next_part, u, t):
"""
Return the log-pdf value for the possible future state 'next'
given input u
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- next_part (array-like): particle estimate for t+1
- u (array-like): input signal
- t (float): time stamps
Returns:
(array-like) with first dimension = N, logp(x_{t+1}|x_t^i)
"""
f = self.calc_f(particles, u, t)
if (f is None):
f = self.f
diff = next_part - f
Q = self.calc_Q(particles, u, t)
if (Q is None):
if (self.Qcholtri.shape[0] == 1):
lpx = kalman.lognormpdf_scalar(diff, self.Qcholtri)
else:
lpx = kalman.lognormpdf_cho_vec(diff, self.Qchol)
else:
N = len(particles)
lpx = numpy.empty(N)
for i in xrange(N):
Qchol = scipy.linalg.cho_factor(Q[i], check_finite=False)
lpx[i] = kalman.lognormpdf_cho(diff[i], Qchol)
return lpx
def measure(self, particles, y, t):
"""
Return the log-pdf value of the measurement
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- y (array-like): measurement
- t (float): time-stamp
Returns:
(array-like) with first dimension = N, logp(y|x^i)
"""
N = len(particles)
lpy = numpy.empty(N)
g = self.calc_g(particles=particles, t=t)
R = self.calc_R(particles=particles, t=t)
if (g is None):
g = numpy.repeat(self.g.reshape((1, -1, 1)), N, 0)
else:
g = g.reshape((N, -1, 1))
yrep = numpy.repeat(numpy.asarray(y).reshape((1, -1, 1)), N, 0)
diff = yrep - g
if (R is None):
if (self.Rcholtri.shape[0] == 1):
lpy = kalman.lognormpdf_scalar(diff, self.Rcholtri)
else:
lpy = kalman.lognormpdf_cho_vec(diff, self.Rchol)
else:
lpy = numpy.empty(N)
for i in range(N):
Rchol = scipy.linalg.cho_factor(R[i], check_finite=False)
lpy[i] = kalman.lognormpdf_cho(diff[i], Rchol)
return lpy
def measure(self, particles, y, t):
"""
Return the log-pdf value of the measurement
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- y (array-like): measurement
- t (float): time-stamp
Returns:
(array-like) with first dimension = N, logp(y|x^i)
"""
N = len(particles)
lpy = numpy.empty(N)
g = self.calc_g(particles=particles, t=t)
R = self.calc_R(particles=particles, t=t)
if (g is None):
g = numpy.repeat(self.g.reshape((1, -1, 1)), N, 0)
else:
g = g.reshape((N, -1, 1))
yrep = numpy.repeat(numpy.asarray(y).reshape((1, -1, 1)), N, 0)
diff = yrep - g
if (R is None):
if (self.Rcholtri.shape[0] == 1):
lpy = kalman.lognormpdf_scalar(diff, self.Rcholtri)
else:
lpy = kalman.lognormpdf_cho_vec(diff, self.Rchol)
else:
lpy = numpy.empty(N)
for i in xrange(N):
Rchol = scipy.linalg.cho_factor(R[i], check_finite=False)
lpy[i] = kalman.lognormpdf_cho(diff[i], Rchol)
return lpy