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 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
