def predict(self, Q=None, u=None):
""" TODO: write docstring """
x_ = self.state['expected']
P_ = self.state['err_cov']
x = np.atleast_2d(feval(self.process['f'], x_, u))
self.state['expected'] = x.copy()
F_ = np.atleast_2d(feval(self.process['Jacobian_x'], x_, u))
if self.process['Jacobian_Q'] is None:
w_ = np.eye(x_.shape[0])
else:
w_ = np.atleast_2d(feval(self.process['Jacobian_Q'], x_, u))
if Q is None:
Q = self.process['Q']
else:
self.process['Q'] = np.atleast_2d(Q)
P = np.atleast_2d(F_ @ P_ @ F_.T + w_ @ Q @ w_.T)
self.state['err_cov'] = P.copy()
if self._verbose:
print(self.state)
self._history['predictions'].append(self.state.copy())
def __init__(
self,
x0,
P0,
f=None,
Q0=None,
Jf=None,
JQ=None, # Jacobians
Hf=None, # Hessian
h=None,
R0=None,
Jh=None,
JR=None, # Jacobians
Hh=None, # Hessian
_verbose: bool = False):
""" TODO: write docstring """
self.state = {
'expected': np.atleast_2d(x0),
'err_cov': np.atleast_2d(P0)
}
if f is None:
if Jf is None:
f = np.eye(x0.shape[0])
else:
f = lambda x: feval(feval(Jf, x), x)
if Q0 is None:
Q0 = np.zeros((x0.shape[0], x0.shape[0]))
if Jf is None:
Jf = np.eye(x0.shape[0])
self.process = {
'f': f,
'Q': Q0,
'Jacobian_x': Jf,
'Hessian_x': Hf,
'Jacobian_Q': JQ
}
if h is None:
h = np.eye(x0.shape[0])
if R0 is None:
y_ = feval(h, x0)
R0 = np.zeros((y_.shape[0], y_.shape[0]))
if Jh is None:
Jh = np.eye(x0.shape[0])
self.observe = {
'h': h,
'R': R0,
'Jacobian_x': Jh,
'Hessian_x': Hh,
'Jacobian_R': JR
}
self._history = {'predictions': [], 'updates': []}
self._history['updates'].append(self.state)
self.initial = self.state.copy()
self._verbose = _verbose
def update(self, y, R=None, u=None):
""" TODO: write docstring """
if self.observe['Hessian_x'] is None:
# If no Hessian supplied, use first order EKF update
return super().update(Q, u)
x_ = self.state['expected']
P_ = self.state['err_cov']
y_ = np.atleast_2d(feval(self.observe['h'], x_, u))
H_ = feval(self.observe['Jacobian_x'], x_, u)
G_ = feval(self.observe['Hessian_x'], x_, u)
if self.observe['Jacobian_R'] is None:
v_ = np.eye(y_.shape[0])
else:
v_ = feval(self.observe['Jacobian_R'], x_, u)
for i_ in range(y_.shape[0]):
y_[i_, 0] += 0.5 * np.trace(np.squeeze(G_[i_, :, :]) @ P_)
if R is None:
R = self.observe['R']
else:
self.observe['R'] = R
P_xy = np.atleast_2d(P_ @ H_.T)
P_y = np.atleast_2d(H_ @ P_xy + v_ @ R @ v_.T)
for i_ in range(y_.shape[0]):
for j_ in range(y_.shape[0]):
P_y[i_, j_] += 0.5 * np.trace(
np.squeeze(G_[i_, :, :]) @ P_ @ np.squeeze(G_[j_, :, :])
@ P_)
K = P_xy @ linalg.inv(P_y)
x = x_ + K @ np.atleast_2d(y - y_)
self.state['expected'] = x.copy()
P = P_ - K @ P_y @ K.T
self.state['err_cov'] = P.copy()
self._history['updates'].append(self.state.copy())
def update(self, y, R=None, u=None):
""" TODO: write docstring """
x_ = self.state['expected']
P_ = self.state['err_cov']
# if u is None:
# y_ = np.atleast_2d(feval(self.observe['h'], x_))
# if self._verbose:
# print(y_)
# H_ = feval(self.observe['Jacobian_x'], x_)
# if self.observe['Jacobian_R'] is None:
# v_ = np.eye(y_.shape[0])
# else:
# v_ = feval(self.observe['Jacobian_R'], x_)
# else:
y_ = np.atleast_2d(feval(self.observe['h'], x_, u))
H_ = feval(self.observe['Jacobian_x'], x_, u)
if self.observe['Jacobian_R'] is None:
v_ = np.eye(y_.shape[0])
else:
v_ = feval(self.observe['Jacobian_R'], x_, u)
if R is None:
R = self.observe['R']
else:
self.observe['R'] = R
P_xy = P_ @ H_.T
P_y = H_ @ P_xy + v_ @ R @ v_.T
K = P_xy @ linalg.inv(P_y)
# The reshape part fixes some weird scalar issues
x = x_ + K @ np.atleast_2d(y - y_)
self.state['expected'] = x.copy()
if self._verbose:
print('x is ' + str(x))
P = P_ - K @ P_y @ K.T
self.state['err_cov'] = P.copy()
self._history['updates'].append(self.state.copy())
def predict(self, Q=None, u=None):
""" TODO: write docstring """
if self.process['Hessian_x'] is None:
# If no Hessian supplied, use first order EKF prediction
return super().predict(Q, u)
x_ = self.state['expected']
P_ = self.state['err_cov']
F_ = feval(self.process['Jacobian_x'], x_, u)
G_ = feval(self.process['Hessian_x'], x_, u)
if self.process['Jacobian_Q'] is None:
w_ = np.eye(x_.shape[0])
else:
w_ = feval(self.process['Jacobian_Q'], x_, u)
x = feval(self.process['f'], x_, u)
for i_ in range(x_.shape[0]):
x[i_, 0] += 0.5 * np.trace(np.squeeze(G_[i_, :, :]) @ P_)
self.state['expected'] = x.copy()
if Q is None:
Q = self.process['Q']
else:
self.process['Q'] = Q
P = F_ @ P_ @ F_.T + w_ @ Q @ w_.T
for i_ in range(x_.shape[0]):
for j_ in range(x_.shape[0]):
P[i_, j_] += 0.5 * np.trace(
np.squeeze(G_[i_, :, :]) @ P_ @ np.squeeze(G_[j_, :, :]))
self.state['err_cov'] = P.copy()
if self._verbose:
print(self.state)
self._history['predictions'].append(self.state.copy())
def smooth_incremental(self, Q=None, u=None):
""" TODO: write docstring """
if Q is None:
Q = self.process['Q']
else:
self.process['Q'] = Q
x_ = self.filtered[self._tx]['expected']
# if u is None:
# x_p = feval(self.process['f'], x_)
# F_ = feval(self.process['Jacobian_x'], x_)
# if self.process['Jacobian_Q'] is None:
# w_ = np.eye(x_.shape[0])
# else:
# w_ = feval(self.process['Jacobian_Q'], x_)
# else:
x_p = feval(self.process['f'], x_, u)
F_ = feval(self.process['Jacobian_x'], x_, u)
if self.process['Jacobian_Q'] is None:
w_ = np.eye(x_.shape[0])
else:
w_ = feval(self.process['Jacobian_Q'], x_, u)
P_ = self.filtered[self._tx]['err_cov']
P_p = F_ @ P_ @ F_.T + w_ @ Q @ w_.T
S = P_ @ F_ @ linalg.inv(P_p)
x_t = self.smoothed[self._tx + 1]['expected']
P_t = self.smoothed[self._tx + 1]['err_cov']
x = x_ + S @ np.atleast_2d(x_t - x_p)
P = P_ + S @ np.atleast_2d(P_t - P_p) @ S.T
self.smoothed[self._tx] = {'expected': x, 'err_cov': P}
self._tx -= 1
def __init__(self,
x0,
P0,
f=None,
Q0=None,
h=None,
R0=None,
params=unscented_default_params(),
augment: bool = False,
_verbose: bool = False):
""" TODO: write docstring """
self.state = {
'expected': np.atleast_2d(x0),
'err_cov': np.atleast_2d(P0)
}
if f is None:
f = np.eye(x0.shape[0])
if Q0 is None:
Q0 = np.zeros((x0.shape[0], x0.shape[0]))
self.process = {'f': f, 'Q': Q0}
if h is None:
h = np.eye(x0.shape[0])
if R0 is None:
y_ = feval(h, x0)
R0 = np.zeros((y_.shape[0], y_.shape[0]))
self.observe = {'h': h, 'R': R0}
self.params = params
self._history = {'predictions': [], 'updates': []}
self._history['updates'].append(self.state)
self.initial = self.state.copy()
self.augment = augment
self._verbose = _verbose