def analytical_smoothing_cost(traj, measurements, m_1_0, P_1_0,
motion_model: MotionModel,
meas_model: MeasModel):
"""Cost function for an optimisation problem used in the family of extended smoothers
Efficient implementation which assumes that the motion and meas models have no explicit dependency on the time step.
GN optimisation of this cost function will result in a linearised function
corresponding to the Iterated Extended Kalman Smoother (IEKS).
Args:
traj: states for a time sequence 1, ..., K
represented as a np.array(K, D_x).
(The actual variable in the cost function)
measurements: measurements for a time sequence 1, ..., K
represented as a list of length K of np.array(D_y,)
"""
K = len(measurements)
prior_diff = traj[0, :] - m_1_0
_cost = prior_diff.T @ np.linalg.inv(P_1_0) @ prior_diff
motion_diff = traj[1:, :] - motion_model.map_set(traj[:-1, :], None)
meas_diff = measurements - meas_model.map_set(traj, None)
for k in range(0, K - 1):
_cost += motion_diff[k, :].T @ np.linalg.inv(
motion_model.proc_noise(k)) @ motion_diff[k, :]
# measurements are zero indexed, i.e. k-1 --> y_k
if any(np.isnan(meas_diff[k, :])):
continue
_cost += meas_diff[k, :].T @ np.linalg.inv(
meas_model.meas_noise(k)) @ meas_diff[k, :]
_cost += meas_diff[-1, :].T @ np.linalg.inv(
meas_model.meas_noise(K)) @ meas_diff[-1, :]
return _cost
def gen_measurements(states: np.ndarray, standard_normal_noise: np.ndarray,
meas_model: MeasModel):
num_time_steps = states.shape[0]
standard_normal_noise = standard_normal_noise.reshape((num_time_steps, 1))
time_step = None
std = meas_model.meas_noise(time_step)
noise = std * standard_normal_noise
return meas_model.map_set(states, time_step) + noise
def grad_analytical_smoothing_cost(x_0, measurements, m_1_0, P_1_0,
motion_model: MotionModel,
meas_model: MeasModel):
"""Gradient of the cost function f in `analytical_smoothing_cost`
Here, the full trajectory x_1:K is interpreted as one vector (x_1^T, ..., x_K)^T with K d_x elements.
Args:
x_0: current iterate
represented as a np.array(K, D_x).
measurements: measurements for a time sequence 1, ..., K
represented as a list of length K of np.array(D_y,)
"""
K = len(measurements)
D_x = m_1_0.shape[0]
grad_pred = np.zeros((K * D_x, ))
grad_update = np.zeros(grad_pred.shape)
prior_diff = x_0[0, :] - m_1_0
motion_diff = x_0[1:, :] - motion_model.map_set(x_0[:-1, :], None)
grad_pred[0:D_x] = np.linalg.inv(P_1_0) @ prior_diff
F_1 = motion_model.jacobian(x_0[0, :], 1)
grad_update[0:D_x] = F_1.T @ np.linalg.inv(
motion_model.proc_noise(1)) @ motion_diff[0, :]
for k_ind in range(1, K - 1):
k = k_ind + 1
grad_pred[k_ind * D_x:(k_ind + 1) * D_x] = (np.linalg.inv(
motion_model.proc_noise(k - 1)) @ motion_diff[k_ind - 1, :])
F_k = motion_model.jacobian(x_0[k_ind, :], k)
grad_update[k_ind * D_x:(k_ind + 1) * D_x] = (F_k.T @ np.linalg.inv(
motion_model.proc_noise(k)) @ motion_diff[k_ind, :])
grad_update[0:D_x] = np.linalg.inv(
motion_model.proc_noise(K - 1)) @ motion_diff[-1, :]
meas_diff = measurements - meas_model.map_set(x_0, None)
grad_meas = np.zeros(grad_pred.shape)
for k_ind in range(0, K):
k = k_ind + 1
if any(np.isnan(meas_diff[k_ind, :])):
continue
H_k = meas_model.jacobian(x_0[k_ind, :], k)
R_k_inv = np.linalg.inv(meas_model.meas_noise(k))
grad_meas[k_ind * D_x:(k_ind + 1) *
D_x] = H_k.T @ R_k_inv @ meas_diff[k_ind, :]
return grad_pred - grad_meas # - grad_update
def simulate_data(
num_steps: int,
prior_mean: float,
prior_cov: float,
motion_model: NonStationaryGrowth,
meas_model: MeasModel,
) -> Tuple[np.ndarray, np.ndarray]:
chol_ini = np.sqrt(prior_cov)
chol_Q = np.sqrt(motion_model.proc_noise(None))
states = np.empty((num_steps, 1))
xk = prior_mean + chol_ini * np.random.randn()
for k in range(1, num_steps):
xk_pred = motion_model.mapping(xk, k) + chol_Q * np.random.randn()
states[k] = xk_pred
xk = xk_pred
chol_R = np.sqrt(meas_model.meas_noise(None))
meas_noise = chol_R * np.random.randn(num_steps, 1)
meas = meas_model.map_set(states) + meas_noise
return states, meas
def dir_der_analytical_smoothing_cost(x_0, p, measurements, m_1_0, P_1_0,
motion_model: MotionModel,
meas_model: MeasModel):
"""Directional derivative of the cost function f in `analytical_smoothing_cost`
Here, the full trajectory x_1:K is interpreted as one vector (x_1^T, ..., x_K)^T with K d_x elements.
Args:
x_0: current iterate
represented as a np.array(K, D_x).
p: search direction, here: x_1 - x_0, i.e. new smoothing estimated means minus current iterate.
represented as a np.array(K, D_x).
measurements: measurements for a time sequence 1, ..., K
represented as a list of length K of np.array(D_y,)
"""
K = len(measurements)
prior_diff = x_0[0, :] - m_1_0
motion_diff = x_0[1:, :] - motion_model.map_set(x_0[:-1, :], None)
meas_diff = measurements - meas_model.map_set(x_0, None)
der = p[0, :] @ np.linalg.inv(P_1_0) @ prior_diff
H_1 = meas_model.jacobian(x_0[0, :], 1)
R_1_inv = np.linalg.inv(meas_model.meas_noise(1))
der -= p[0, :] @ H_1.T @ R_1_inv @ meas_diff[0, :]
for k_ind in range(1, K):
k = k_ind + 1
F_k_min_1 = motion_model.jacobian(x_0[k_ind - 1, :], k - 1)
factor_1 = p[k_ind, :].T - F_k_min_1 @ p[k_ind - 1, :].T
der += factor_1 @ np.linalg.inv(
motion_model.proc_noise(k - 1)) @ motion_diff[k_ind - 1, :]
if any(np.isnan(meas_diff[k_ind, :])):
continue
H_k = meas_model.jacobian(x_0[k_ind, :], k)
R_k_inv = np.linalg.inv(meas_model.meas_noise(k))
der -= p[k_ind, :] @ H_k.T @ R_k_inv @ meas_diff[k_ind, :]
return der
def analytical_smoothing_cost_lm_ext(traj, measurements, prev_means, m_1_0,
P_1_0, motion_model: MotionModel,
meas_model: MeasModel, lambda_):
"""Cost function for an optimisation problem used in the family of extended smoothers
with LM regularisation
GN optimisation of this cost function will result in a linearised function
corresponding to the Levenberg-Marquardt Iterated Extended Kalman Smoother (IEKS)
Args:
traj: states for a time sequence 1, ..., K
represented as a np.array(K, D_x).
(The actual variable in the cost function)
measurements: measurements for a time sequence 1, ..., K
represented as a list of length K of np.array(D_y,)
"""
prior_diff = traj[0, :] - m_1_0
_cost = prior_diff.T @ np.linalg.inv(P_1_0) @ prior_diff
motion_diff = traj[1:, :] - motion_model.map_set(traj[:-1, :], None)
meas_diff = measurements - meas_model.map_set(traj, None)
for k in range(0, traj.shape[0] - 1):
_cost += motion_diff[k, :].T @ np.linalg.inv(
motion_model.proc_noise(k)) @ motion_diff[k, :]
# measurements are zero indexed, i.e. k-1 --> y_k
if any(np.isnan(meas_diff[k, :])):
continue
_cost += meas_diff[k, :].T @ np.linalg.inv(
meas_model.meas_noise(k)) @ meas_diff[k, :]
_cost += meas_diff[-1, :].T @ np.linalg.inv(
meas_model.meas_noise(measurements.shape[0])) @ meas_diff[-1, :]
lm_dist = _lm_ext(traj, prev_means, lambda_)
_cost += lm_dist
return _cost