def test_optimize_1D(self):
# test on simple 1D example, plot the fit
steps = 100
dyn = UNGMTransition(GaussRV(1), GaussRV(1))
x, y = dyn.simulate_discrete(steps)
f = dyn.dyn_eval
dim_in, dim_out = dyn.dim_in, dyn.dim_state
par0 = 1 + np.random.rand(dim_out, dim_in + 1)
tf = MultiOutputGaussianProcessTransform(dim_in, dim_out, par0)
# use sampled system state trajectory to create training data
fy = np.zeros((dim_out, steps))
for k in range(steps):
fy[:, k] = f(x[:, k, 0], k)
b = [np.log((0.1, 1.0001))] + dim_in * [(None, None)]
opt = {'xtol': 1e-2, 'maxiter': 100}
log_par, res_list = tf.model.optimize(np.log(par0),
fy,
x[..., 0],
bounds=b,
method='L-BFGS-B',
options=opt)
print(np.exp(log_par))
self.assertTrue(False)
def test_optimize(self):
steps = 350
m0 = np.array([1000, 300, 1000, 0, np.deg2rad(-3)])
P0 = np.diag([100, 10, 100, 10, 0.1])
x0 = GaussRV(5, m0, P0)
dt = 0.1
Q = np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]])
q = GaussRV(5, cov=Q)
dyn = CoordinatedTurnTransition(x0, q, dt)
x, y = dyn.simulate_discrete(steps)
f = dyn.dyn_eval
dim_in, dim_out = dyn.dim_in, dyn.dim_state
# par0 = np.hstack((np.ones((dim_out, 1)), 5*np.ones((dim_out, dim_in+1))))
par0 = 10 * np.ones((dim_out, dim_in + 1))
tf = MultiOutputGaussianProcessTransform(dim_in, dim_out, par0)
# use sampled system state trajectory to create training data
fy = np.zeros((dim_out, steps))
for k in range(steps):
fy[:, k] = f(x[:, k, 0], 0)
opt = {'maxiter': 100}
log_par, res_list = tf.model.optimize(np.log(par0),
fy,
x[..., 0],
method='BFGS',
options=opt)
print(np.exp(log_par))
def hypers_demo(lscale=None):
print(f"Seed = {np.random.get_state()[1][0]}")
# set default lengthscales if unspecified
if lscale is None:
lscale = [1e-3, 3e-3, 1e-2, 3e-2, 1e-1, 3e-1, 1, 3, 1e1, 3e1, 1e2, ]
steps, mc = 500, 100
# setup univariate non-stationary growth model
x0 = GaussRV(1, cov=np.atleast_2d(5.0))
q = GaussRV(1, cov=np.atleast_2d(10.0))
dyn = UNGMTransition(x0, q) # dynamics
r = GaussRV(1)
obs = UNGMMeasurement(r, 1) # observation model
x = dyn.simulate_discrete(steps, mc_sims=mc) # generate some data
z = obs.simulate_measurements(x)
num_el = len(lscale)
mean_f, cov_f = np.zeros((dyn.dim_in, steps, mc, num_el)), np.zeros((dyn.dim_in, dyn.dim_in, steps, mc, num_el))
for iel, el in enumerate(lscale):
# kernel parameters
ker_par = np.array([[1.0, el * dyn.dim_in]])
# initialize BHKF with current lenghtscale
f = GaussianProcessKalman(dyn, obs, ker_par, ker_par, points='ut', point_hyp={'kappa': 0.0})
# filtering
for s in range(mc):
mean_f[..., s, iel], cov_f[..., s, iel] = f.forward_pass(z[..., s])
# evaluate RMSE, NCI and NLL
rmseVsEl = squared_error(x[..., na], mean_f)
nciVsEl = rmseVsEl.copy()
nllVsEl = rmseVsEl.copy()
for k in range(steps):
for iel in range(num_el):
mse_mat = mse_matrix(x[:, k, :], mean_f[:, k, :, iel])
for s in range(mc):
nciVsEl[:, k, s, iel] = log_cred_ratio(x[:, k, s], mean_f[:, k, s, iel], cov_f[:, :, k, s, iel],
mse_mat)
nllVsEl[:, k, s, iel] = neg_log_likelihood(x[:, k, s], mean_f[:, k, s, iel], cov_f[:, :, k, s, iel])
# average out time and MC simulations
rmseVsEl = np.sqrt(np.mean(rmseVsEl, axis=1)).mean(axis=1)
nciVsEl = nciVsEl.mean(axis=(1, 2))
nllVsEl = nllVsEl.mean(axis=(1, 2))
# plot influence of changing lengthscale on the RMSE and NCI and NLL filter performance
plt.figure()
plt.semilogx(lscale, rmseVsEl.squeeze(), color='k', ls='-', lw=2, marker='o', label='RMSE')
plt.semilogx(lscale, nciVsEl.squeeze(), color='k', ls='--', lw=2, marker='o', label='NCI')
plt.semilogx(lscale, nllVsEl.squeeze(), color='k', ls='-.', lw=2, marker='o', label='NLL')
plt.grid(True)
plt.legend()
plt.show()
return {'el': lscale, 'rmse': rmseVsEl, 'nci': nciVsEl, 'neg_log_likelihood': nllVsEl}
def test_crash(self):
d = 1
tmc = MonteCarloTransform(d, n=1e4)
f = UNGMTransition(GaussRV(1, cov=1.0), GaussRV(1, cov=10.0)).dyn_eval
mean = np.zeros(d)
cov = np.eye(d)
# does it crash ?
tmc.apply(f, mean, cov, np.atleast_1d(1.0))
def test_simulate_continuous(self):
m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
x0 = GaussRV(5, m0, P0)
q = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 1e-6]))
dyn = ReentryVehicle2DTransition(x0, q, dt=0.05)
x = dyn.simulate_continuous(200, dt=0.05)
plt.figure()
plt.plot(x[0, ...], x[1, ...], color='r')
plt.show()
def test_simulate(self):
# setup CTRS with radar measurements
x0 = GaussRV(5, cov=0.1 * np.eye(5))
q = GaussRV(2, cov=np.diag([0.1, 0.1 * np.pi]))
r = GaussRV(2, cov=np.diag([0.3, 0.03]))
dyn = ConstantTurnRateSpeed(x0, q)
obs = Radar2DMeasurement(r, 5)
x = dyn.simulate_discrete(100, 10)
y = obs.simulate_measurements(x)
plt.figure()
plt.plot(x[0, ...], x[1, ...], alpha=0.25, color='b')
plt.show()
def test_cho_dot_ein(self):
# attempt to compute the transformed covariance using cholesky decomposition
# integrand
# input moments
mean_in = np.array([6500.4, 349.14, 1.8093, 6.7967, 0.6932])
cov_in = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
f = ReentryVehicle2DTransition(GaussRV(5, mean_in, cov_in), GaussRV(3)).dyn_eval
dim_in, dim_out = ReentryVehicle2DTransition.dim_state, 1
# transform
ker_par_mo = np.hstack((np.ones((dim_out, 1)), 25 * np.ones((dim_out, dim_in))))
tf_so = GaussianProcessTransform(dim_in, dim_out, ker_par_mo, point_str='sr')
# Monte-Carlo for ground truth
# tf_ut = UnscentedTransform(dim_in)
# tf_ut.apply(f, mean_in, cov_in, np.atleast_1d(1), None)
tf_mc = MonteCarloTransform(dim_in, 1000)
mean_mc, cov_mc, ccov_mc = tf_mc.apply(f, mean_in, cov_in, np.atleast_1d(1))
C_MC = cov_mc + np.outer(mean_mc, mean_mc.T)
# evaluate integrand
x = mean_in[:, None] + la.cholesky(cov_in).dot(tf_so.model.points)
Y = np.apply_along_axis(f, 0, x, 1.0, None)
# covariance via np.dot
iK, Q = tf_so.model.iK, tf_so.model.Q
C1 = iK.dot(Q).dot(iK)
C1 = Y.dot(C1).dot(Y.T)
# covariance via np.einsum
C2 = np.einsum('ab, bc, cd', iK, Q, iK)
C2 = np.einsum('ab,bc,cd', Y, C2, Y.T)
# covariance via np.dot and cholesky
K = tf_so.model.kernel.eval(tf_so.model.kernel.par, tf_so.model.points)
L_lower = la.cholesky(K)
Lq = la.cholesky(Q)
phi = la.solve(L_lower, Lq)
psi = la.solve(L_lower, Y.T)
bet = psi.T.dot(phi)
C3_dot = bet.dot(bet.T)
C3_ein = np.einsum('ij, jk', bet, bet.T)
logging.debug("MAX DIFF: {:.4e}".format(np.abs(C1 - C2).max()))
logging.debug("MAX DIFF: {:.4e}".format(np.abs(C3_dot - C3_ein).max()))
self.assertTrue(np.allclose(C1, C2), "MAX DIFF: {:.4e}".format(np.abs(C1 - C2).max()))
self.assertTrue(np.allclose(C3_dot, C3_ein), "MAX DIFF: {:.4e}".format(np.abs(C3_dot - C3_ein).max()))
self.assertTrue(np.allclose(C1, C3_dot), "MAX DIFF: {:.4e}".format(np.abs(C1 - C3_dot).max()))
def test_simulate(self):
time_steps = 50
# UNGM additive noise
dim = 1
init_dist = GaussRV(dim)
noise_dist = GaussRV(dim, cov=np.atleast_2d(10.0))
ungm_dyn = UNGMTransition(init_dist, noise_dist)
ungm_meas = UNGMMeasurement(GaussRV(dim), ungm_dyn.dim_state)
x = ungm_dyn.simulate_discrete(time_steps, mc_sims=20)
y = ungm_meas.simulate_measurements(x)
# UNGM non-additive noise
ungmna_dyn = UNGMNATransition(init_dist, noise_dist)
ungmna_meas = UNGMNAMeasurement(GaussRV(dim), ungm_dyn.dim_state)
x = ungmna_dyn.simulate_discrete(time_steps, mc_sims=20)
y = ungmna_meas.simulate_measurements(x)
def test_total_nlml_gradient(self):
# nonlinear vector function from some SSM
dyn = CoordinatedTurnTransition(GaussRV(5), GaussRV(5))
# generate inputs
num_x = 20
x = 10 + np.random.randn(dyn.dim_in, num_x)
# evaluate function at inputs
y = np.apply_along_axis(dyn.dyn_eval, 0, x, None)
# kernel and it's initial parameters
from ssmtoybox.bq.bqkern import RBFGauss
lhyp = np.log([1.0] + 5 * [3.0])
kernel = RBFGauss(dyn.dim_in, self.ker_par_5d)
from scipy.optimize import check_grad
err = check_grad(self._total_nlml, self._total_nlml_grad, lhyp, kernel,
y.T, x)
print(err)
self.assertTrue(err <= 1e-5, 'Gradient error: {:.4f}'.format(err))
def test_apply(self):
dyn = Pendulum2DTransition(GaussRV(2), GaussRV(2))
f = dyn.dyn_eval
dim_in, dim_out = dyn.dim_in, dyn.dim_state
ker_par = np.hstack((np.ones((dim_out, 1)), 3 * np.ones(
(dim_out, dim_in))))
tf = MultiOutputGaussianProcessTransform(dim_in, dim_out, ker_par)
mean, cov = np.zeros(dim_in, ), np.eye(dim_in)
tmean, tcov, tccov = tf.apply(f, mean, cov, np.atleast_1d(1.0))
print("Transformed moments\nmean: {}\ncov: {}\nccov: {}".format(
tmean, tcov, tccov))
# test positive definiteness
try:
la.cholesky(tcov)
except la.LinAlgError:
self.fail("Output covariance not positive definite.")
# test symmetry
self.assertTrue(np.allclose(tcov, tcov.T),
"Output covariance not closely symmetric.")
def gpq_int_var_demo():
"""Compares integral variances of GPQ and GPQ+D by plotting."""
d = 1
f = UNGMTransition(GaussRV(d), GaussRV(d)).dyn_eval
mean = np.zeros(d)
cov = np.eye(d)
kpar = np.array([[10.0] + d * [0.7]])
gpq = GaussianProcessTransform(d,
1,
kern_par=kpar,
kern_str='rbf',
point_str='ut',
point_par={'kappa': 0.0})
gpqd = GaussianProcessDerTransform(d,
1,
kern_par=kpar,
point_str='ut',
point_par={'kappa': 0.0})
mct = MonteCarloTransform(d, n=1e4)
mean_gpq, cov_gpq, cc_gpq = gpq.apply(f, mean, cov, np.atleast_1d(1.0))
mean_gpqd, cov_gpqd, cc_gpqd = gpqd.apply(f, mean, cov, np.atleast_1d(1.0))
mean_mc, cov_mc, cc_mc = mct.apply(f, mean, cov, np.atleast_1d(1.0))
xmin_gpq = norm.ppf(0.0001, loc=mean_gpq, scale=gpq.model.integral_var)
xmax_gpq = norm.ppf(0.9999, loc=mean_gpq, scale=gpq.model.integral_var)
xmin_gpqd = norm.ppf(0.0001, loc=mean_gpqd, scale=gpqd.model.integral_var)
xmax_gpqd = norm.ppf(0.9999, loc=mean_gpqd, scale=gpqd.model.integral_var)
xgpq = np.linspace(xmin_gpq, xmax_gpq, 500)
ygpq = norm.pdf(xgpq, loc=mean_gpq, scale=gpq.model.integral_var)
xgpqd = np.linspace(xmin_gpqd, xmax_gpqd, 500)
ygpqd = norm.pdf(xgpqd, loc=mean_gpqd, scale=gpqd.model.integral_var)
plt.figure()
plt.plot(xgpq, ygpq, lw=2, label='gpq')
plt.plot(xgpqd, ygpqd, lw=2, label='gpq+d')
plt.gca().add_line(
Line2D([mean_mc, mean_mc], [0, 150], linewidth=2, color='k'))
plt.legend()
plt.show()
def test_single_vs_multi_output(self):
# results of the GPQ and GPQMO should be same if parameters properly chosen, GPQ is a special case of GPQMO
m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
x0 = GaussRV(5, m0, P0)
dyn = ReentryVehicle2DTransition(x0, GaussRV(5))
f = dyn.dyn_eval
dim_in, dim_out = dyn.dim_in, dyn.dim_state
# input mean and covariance
mean_in, cov_in = m0, P0
# single-output GPQ
ker_par_so = np.hstack((np.ones((1, 1)), 25 * np.ones((1, dim_in))))
tf_so = GaussianProcessTransform(dim_in, dim_out, ker_par_so)
# multi-output GPQ
ker_par_mo = np.hstack((np.ones((dim_out, 1)), 25 * np.ones(
(dim_out, dim_in))))
tf_mo = MultiOutputGaussianProcessTransform(dim_in, dim_out,
ker_par_mo)
# transformed moments
# FIXME: transformed covariances different
mean_so, cov_so, ccov_so = tf_so.apply(f, mean_in, cov_in,
np.atleast_1d(0))
mean_mo, cov_mo, ccov_mo = tf_mo.apply(f, mean_in, cov_in,
np.atleast_1d(0))
print('mean delta: {}'.format(np.abs(mean_so - mean_mo).max()))
print('cov delta: {}'.format(np.abs(cov_so - cov_mo).max()))
print('ccov delta: {}'.format(np.abs(ccov_so - ccov_mo).max()))
# results of GPQ and GPQMO should be the same
self.assertTrue(np.array_equal(mean_so, mean_mo))
self.assertTrue(np.array_equal(cov_so, cov_mo))
self.assertTrue(np.array_equal(ccov_so, ccov_mo))
def test_hypers_optim_multioutput(self):
m0 = np.array([1000, 300, 1000, 0, np.deg2rad(-3)])
P0 = np.diag([100, 10, 100, 10, 0.1])
x0 = GaussRV(5, m0, P0)
dyn = CoordinatedTurnTransition(x0, GaussRV(5))
func = dyn.dyn_eval
dim_in, dim_out = dyn.dim_in, dyn.dim_state
model = StudentTProcessModel(dim_in, self.ker_par_5d, 'rbf',
'ut') # , point_hyp={'degree': 10})
x = m0[:,
na] + model.points # ssm.get_pars('x0_cov')[0].dot(model.points)
y = np.apply_along_axis(func, 0, x, None) # (d_out, n**2)
# lhyp0 = np.log(np.ones((dim_out, dim_in+1)))
lhyp0 = np.log(10 + np.ones((1, dim_in + 1)))
res_ml2 = model.optimize(lhyp0, y.T, model.points, method='BFGS')
hyp_ml2 = np.exp(res_ml2.x)
print(res_ml2)
np.set_printoptions(precision=4)
print('ML-II({:.4f}) @ alpha: {:.4f}, el: {}'.format(
res_ml2.fun, hyp_ml2[0], hyp_ml2[1:]))
def test_radar(self):
r = GaussRV(2)
dim_state = 5
st_ind = np.array([0, 2])
radar_location = np.array([6378.0, 0])
obs = Radar2DMeasurement(r,
dim_state,
state_index=st_ind,
radar_loc=radar_location)
st, n = np.random.randn(5), np.random.randn(2)
# check evaluation of the measurement function
hx = obs.meas_eval(st, n, dx=False)
self.assertTrue(hx.shape == (2, ))
jac = obs.meas_eval(st, n, dx=True)
# check proper dimensions
self.assertEqual(jac.shape, (2, 5))
# non-zero columns only at state_indexes
self.assertTrue(np.array_equal(np.nonzero(jac.sum(axis=0))[0], st_ind))
def setUpClass(cls):
# setup UNGM
x0 = GaussRV(1, cov=np.atleast_2d(1.0))
q = GaussRV(1, cov=np.atleast_2d(10.0))
cls.dyn_ungm = UNGMTransition(x0, q)
r = GaussRV(1, cov=np.atleast_2d(1.0))
cls.obs_ungm = UNGMMeasurement(r, 1)
# setup pendulum
dt = 0.01
x0 = GaussRV(2, np.array([1.5, 0]), 0.01 * np.eye(2))
q = GaussRV(2, cov=np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]))
cls.dyn_pend = Pendulum2DTransition(x0, q, dt)
r = GaussRV(1, cov=np.atleast_2d(0.1))
cls.obs_pend = Pendulum2DMeasurement(r, cls.dyn_pend.dim_state)
def setUpClass(cls):
cls.ssm = {}
# setup UNGM
x0 = GaussRV(1)
q = GaussRV(1, cov=np.array([[10.0]]))
r = GaussRV(1)
dyn = UNGMTransition(x0, q)
obs = UNGMMeasurement(r, 1)
x = dyn.simulate_discrete(100)
y = obs.simulate_measurements(x)
cls.ssm.update({'ungm': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}})
# setup UNGM with non-additive noise
x0 = GaussRV(1)
q = GaussRV(1, cov=np.array([[10.0]]))
r = GaussRV(1)
dyn = UNGMNATransition(x0, q)
obs = UNGMNAMeasurement(r, 1)
x = dyn.simulate_discrete(100)
y = obs.simulate_measurements(x)
cls.ssm.update({'ungmna': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}})
# setup 2D pendulum
x0 = GaussRV(2, mean=np.array([1.5, 0]), cov=0.01 * np.eye(2))
dt = 0.01
q = GaussRV(2,
cov=0.01 * np.array([[(dt**3) / 3,
(dt**2) / 2], [(dt**2) / 2, dt]]))
r = GaussRV(1, cov=np.array([[0.1]]))
dyn = Pendulum2DTransition(x0, q, dt=dt)
obs = Pendulum2DMeasurement(r, dyn.dim_state)
x = dyn.simulate_discrete(100)
y = obs.simulate_measurements(x)
cls.ssm.update({'pend': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}})
# setup reentry vehicle radar tracking
m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
x0 = GaussRV(5, m0, P0)
q = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 1e-6]))
r = GaussRV(2, cov=np.diag([1e-6, 0.17e-6]))
dyn = ReentryVehicle2DTransition(x0, q)
obs = Radar2DMeasurement(r, 5)
x = dyn.simulate_discrete(100)
y = obs.simulate_measurements(x)
cls.ssm.update({'rer': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}})
# setup coordinated turn bearing only tracking
m0 = np.array([1000, 300, 1000, 0, np.deg2rad(-3.0)])
P0 = np.diag([100, 10, 100, 10, 0.1])
x0 = GaussRV(5, m0, P0)
dt = 0.1
rho_1, rho_2 = 0.1, 1.75e-4
A = np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]])
Q = np.zeros((5, 5))
Q[:2, :2], Q[2:4, 2:4], Q[4, 4] = rho_1 * A, rho_1 * A, rho_2 * dt
q = GaussRV(5, cov=Q)
r = GaussRV(4, cov=10e-3 * np.eye(4))
sen = np.vstack((1000 * np.eye(2), -1000 * np.eye(2))).astype(np.float)
dyn = CoordinatedTurnTransition(x0, q)
obs = BearingMeasurement(r, 5, state_index=[0, 2], sensor_pos=sen)
x = dyn.simulate_discrete(100)
y = obs.simulate_measurements(x)
cls.ssm.update({'ctb': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}})
# setup CTRS with radar measurements
x0 = GaussRV(5, cov=0.1 * np.eye(5))
q = GaussRV(2, cov=np.diag([0.1, 0.1 * np.pi]))
r = GaussRV(2, cov=np.diag([0.3, 0.03]))
dyn = ConstantTurnRateSpeed(x0, q)
obs = Radar2DMeasurement(r, 5)
x = dyn.simulate_discrete(100)
y = obs.simulate_measurements(x)
cls.ssm.update({'ctrs': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}})
import time import numpy as np import pandas as pd from research.gpq.icinco_demo import evaluate_performance from ssmtoybox.mtran import UnscentedTransform from ssmtoybox.ssinf import ExtendedKalman, ExtendedKalmanGPQD from ssmtoybox.ssmod import UNGMTransition, UNGMMeasurement from ssmtoybox.utils import GaussRV steps, mc = 50, 10 # time steps, mc simulations # setup univariate non-stationary growth model x0 = GaussRV(1, cov=np.atleast_2d(5.0)) q = GaussRV(1, cov=np.atleast_2d(10.0)) dyn = UNGMTransition(x0, q) # dynamics r = GaussRV(1) obs = UNGMMeasurement(r, 1) # observation model x = dyn.simulate_discrete(steps, mc) z = obs.simulate_measurements(x) # use only the central sigma-point usp_0 = np.zeros((dyn.dim_in, 1)) usp_ut = UnscentedTransform.unit_sigma_points(dyn.dim_in) # set the RBF kernel hyperparameters hyp_rbf = np.array([[1.0] + dyn.dim_in * [3.0]]) hyp_rbf_ut = np.array([[8.0] + dyn.dim_in * [0.5]])
def ukf_trunc_demo(mc_sims=50):
disc_tau = 0.5 # discretization period in seconds
duration = 200
# define system
m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 0])
x0 = GaussRV(5, m0, P0)
q = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 0]))
sys = ReentryVehicle2DTransition(x0, q, dt=disc_tau)
# define radar measurement model
r = GaussRV(2, cov=np.diag([1e-6, 0.17e-6]))
obs = Radar2DMeasurement(r, sys.dim_state)
# simulate reference state trajectory by SDE integration
x = sys.simulate_continuous(duration, disc_tau, mc_sims)
x_ref = x.mean(axis=2)
# simulate corresponding radar measurements
y = obs.simulate_measurements(x)
# initialize state-space model; uses cartesian2polar as measurement (not polar2cartesian)
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
x0 = GaussRV(5, m0, P0)
q = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 1e-6]))
dyn = ReentryVehicle2DTransition(x0, q, dt=disc_tau)
# initialize UKF and UKF in truncated version
alg = (
UnscentedKalman(dyn, obs),
TruncatedUnscentedKalman(dyn, obs),
)
num_alg = len(alg)
# space for filtered mean and covariance
steps = x.shape[1]
x_mean = np.zeros((dyn.dim_in, steps, mc_sims, num_alg))
x_cov = np.zeros((dyn.dim_in, dyn.dim_in, steps, mc_sims, num_alg))
# filtering estimate of the state trajectory based on provided measurements
from tqdm import trange
for i_est, estimator in enumerate(alg):
for i_mc in trange(mc_sims):
x_mean[..., i_mc, i_est], x_cov[..., i_mc, i_est] = estimator.forward_pass(y[..., i_mc])
estimator.reset()
# Plots
plt.figure()
g = GridSpec(2, 4)
plt.subplot(g[:, :2])
# Earth surface w/ radar position
radar_x, radar_y = dyn.R0, 0
t = 0.02 * np.arange(-1, 4, 0.1)
plt.plot(dyn.R0 * np.cos(t), dyn.R0 * np.sin(t), color='darkblue', lw=2)
plt.plot(radar_x, radar_y, 'ko')
plt.plot(x_ref[0, :], x_ref[1, :], color='r', ls='--')
# Convert from polar to cartesian
meas = np.stack(( + y[0, ...] * np.cos(y[1, ...]), radar_y + y[0, ...] * np.sin(y[1, ...])), axis=0)
for i in range(mc_sims):
# Vehicle trajectory
# plt.plot(x[0, :, i], x[1, :, i], alpha=0.35, color='r', ls='--')
# Plot measurements
plt.plot(meas[0, :, i], meas[1, :, i], 'k.', alpha=0.3)
# Filtered position estimate
plt.plot(x_mean[0, 1:, i, 0], x_mean[1, 1:, i, 0], color='g', alpha=0.3)
plt.plot(x_mean[0, 1:, i, 1], x_mean[1, 1:, i, 1], color='orange', alpha=0.3)
# Performance score plots
error2 = x_mean.copy()
lcr = np.zeros((steps, mc_sims, num_alg))
for a in range(num_alg):
for k in range(steps):
mse = mse_matrix(x[:4, k, :], x_mean[:4, k, :, a])
for imc in range(mc_sims):
error2[:, k, imc, a] = squared_error(x[:, k, imc], x_mean[:, k, imc, a])
lcr[k, imc, a] = log_cred_ratio(x[:4, k, imc], x_mean[:4, k, imc, a], x_cov[:4, :4, k, imc, a], mse)
# Averaged RMSE and Inclination Indicator in time
pos_rmse_vs_time = np.sqrt((error2[:2, ...]).sum(axis=0)).mean(axis=1)
inc_ind_vs_time = lcr.mean(axis=1)
# Plots
plt.subplot(g[0, 2:])
plt.title('RMSE')
plt.plot(pos_rmse_vs_time[:, 0], label='UKF', color='g')
plt.plot(pos_rmse_vs_time[:, 1], label='UKF-trunc', color='r')
plt.legend()
plt.subplot(g[1, 2:])
plt.title('Inclination Indicator $I^2$')
plt.plot(inc_ind_vs_time[:, 0], label='UKF', color='g')
plt.plot(inc_ind_vs_time[:, 1], label='UKF-trunc', color='r')
plt.legend()
plt.show()
print('Average RMSE: {}'.format(pos_rmse_vs_time.mean(axis=0)))
print('Average I2: {}'.format(inc_ind_vs_time.mean(axis=0)))
def setUpClass(cls):
cls.models = []
cls.models.append(UNGMTransition(GaussRV(1), GaussRV(1)))
cls.models.append(Pendulum2DTransition(GaussRV(2), GaussRV(2)))
def tables():
steps, mc = 500, 100
# initialize UNGM model
dyn = UNGMTransition(GaussRV(1, cov=5.0), GaussRV(1, cov=10.0))
obs = UNGMMeasurement(GaussRV(1, cov=1.0), 1)
# generate some data
np.random.seed(0)
x = dyn.simulate_discrete(steps, mc)
z = obs.simulate_measurements(x)
par_ut = np.array([[3.0, 0.3]])
par_gh5 = np.array([[5.0, 0.6]])
par_gh7 = np.array([[3.0, 0.4]])
mulind_ut = np.array([[0, 1, 2]])
mulind_gh = lambda degree: np.atleast_2d(np.arange(degree))
# initialize filters/smoothers
algorithms = (
# Classical filters
UnscentedKalman(dyn, obs, alpha=1.0, beta=0.0),
GaussHermiteKalman(dyn, obs, deg=5),
GaussHermiteKalman(dyn, obs, deg=7),
# GPQ filters
GaussianProcessKalman(dyn,
obs,
par_ut,
par_ut,
kernel='rbf',
points='ut',
point_hyp={'alpha': 1.0}),
GaussianProcessKalman(dyn,
obs,
par_gh5,
par_gh5,
kernel='rbf',
points='gh',
point_hyp={'degree': 5}),
GaussianProcessKalman(dyn,
obs,
par_gh7,
par_gh7,
kernel='rbf',
points='gh',
point_hyp={'degree': 7}),
# BSQ filters
BayesSardKalman(dyn,
obs,
par_ut,
par_ut,
mulind_ut,
mulind_ut,
points='ut',
point_hyp={'alpha': 1.0}),
BayesSardKalman(dyn,
obs,
par_gh5,
par_gh5,
mulind_gh(5),
mulind_gh(5),
points='gh',
point_hyp={'degree': 5}),
BayesSardKalman(dyn,
obs,
par_gh7,
par_gh7,
mulind_gh(7),
mulind_gh(7),
points='gh',
point_hyp={'degree': 7}),
)
num_algs = len(algorithms)
# space for estimates
dim = dyn.dim_state
mean_f, cov_f = np.zeros((dim, steps, mc, num_algs)), np.zeros(
(dim, dim, steps, mc, num_algs))
mean_s, cov_s = np.zeros((dim, steps, mc, num_algs)), np.zeros(
(dim, dim, steps, mc, num_algs))
# do filtering/smoothing
t0 = time.time() # measure execution time
print('Running filters/smoothers ...')
for a, alg in enumerate(algorithms):
print('{}'.format(
alg.__class__.__name__)) # print filter/smoother name
for sim in range(mc):
mean_f[..., sim, a], cov_f[..., sim, a] = alg.forward_pass(z[...,
sim])
mean_s[..., sim, a], cov_s[..., sim, a] = alg.backward_pass()
alg.reset()
print('Done in {0:.4f} [sec]'.format(time.time() - t0))
# evaluate perfomance
scores = evaluate_performance(x, mean_f, cov_f, mean_s, cov_s)
rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s = scores[:
6]
rmseStd_f, nciStd_f, nllStd_f, rmseStd_s, nciStd_s, nllStd_s = scores[6:]
# put data into Pandas DataFrame for fancy printing and latex export
# row_labels = ['SR', 'UT', 'GH-5', 'GH-7', 'GH-10', 'GH-15', 'GH-20']
row_labels = ['UT', 'GH-5', 'GH-7']
num_labels = len(row_labels)
col_labels = [
'Classical', 'GPQ', 'BSQ', 'Classical (2std)', 'GPQ (2std)',
'BSQ (2std)'
]
rmse_table_f = pd.DataFrame(np.hstack(
(rmseMean_f.reshape(3,
num_labels).T, rmseStd_f.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
nci_table_f = pd.DataFrame(np.hstack(
(nciMean_f.reshape(3, num_labels).T, nciStd_f.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
nll_table_f = pd.DataFrame(np.hstack(
(nllMean_f.reshape(3, num_labels).T, nllStd_f.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
rmse_table_s = pd.DataFrame(np.hstack(
(rmseMean_s.reshape(3,
num_labels).T, rmseStd_s.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
nci_table_s = pd.DataFrame(np.hstack(
(nciMean_s.reshape(3, num_labels).T, nciStd_s.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
nll_table_s = pd.DataFrame(np.hstack(
(nllMean_s.reshape(3, num_labels).T, nllStd_s.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
# print kernel parameters
print('Kernel parameters')
print('{:5}: {}'.format('UT', par_ut))
print('{:5}: {}'.format('GH-5', par_gh5))
print('{:5}: {}'.format('GH-7', par_gh7))
print()
# print tables
pd.set_option('precision', 2, 'display.max_columns', 6)
print('Filter RMSE')
print(rmse_table_f)
print('Filter NCI')
print(nci_table_f)
print('Filter NLL')
print(nll_table_f)
print('Smoother RMSE')
print(rmse_table_s)
print('Smoother NCI')
print(nci_table_s)
print('Smoother NLL')
print(nll_table_s)
# return computed metrics for filters and smoothers
return {
'filter_RMSE': rmse_table_f,
'filter_NCI': nci_table_f,
'filter_NLL': nll_table_f,
'smoother_RMSE': rmse_table_s,
'smoother_NCI': nci_table_s,
'smoother_NLL': nll_table_s
}
def lengthscale_filter_demo(lscale):
steps, mc = 500, 20
# initialize UNGM model
dyn = UNGMTransition(GaussRV(1, cov=5.0), GaussRV(1, cov=10.0))
obs = UNGMMeasurement(GaussRV(1, cov=1.0), 1)
# generate some data
x = dyn.simulate_discrete(steps, mc)
z = obs.simulate_measurements(x)
dim = dyn.dim_state
num_el = len(lscale)
# lscale = [1e-3, 3e-3, 1e-2, 3e-2, 1e-1, 3e-1, 1, 3, 1e1, 3e1] # , 1e2, 3e2]
mean_f, cov_f = np.zeros((dim, steps, mc, num_el)), np.zeros(
(dim, dim, steps, mc, num_el))
for iel, el in enumerate(lscale):
# kernel parameters
ker_par = np.array([[1.0, el * dim]])
# initialize BHKF with current lenghtscale
f = GaussianProcessKalman(dyn,
obs,
ker_par,
ker_par,
kernel='rbf',
points='ut')
# filtering
for s in range(mc):
mean_f[..., s, iel], cov_f[..., s, iel] = f.forward_pass(z[..., s])
# evaluate RMSE, NCI and NLL
rmseVsEl = squared_error(x[..., na], mean_f)
nciVsEl = rmseVsEl.copy()
nllVsEl = rmseVsEl.copy()
for k in range(steps):
for iel in range(num_el):
mse_mat = mse_matrix(x[:, k, :], mean_f[:, k, :, iel])
for s in range(mc):
nciVsEl[:, k, s,
iel] = log_cred_ratio(x[:, k, s], mean_f[:, k, s, iel],
cov_f[:, :, k, s, iel], mse_mat)
nllVsEl[:, k, s,
iel] = neg_log_likelihood(x[:, k, s], mean_f[:, k, s,
iel],
cov_f[:, :, k, s, iel])
# average out time and MC simulations
rmseVsEl = np.sqrt(np.mean(rmseVsEl, axis=1)).mean(axis=1)
nciVsEl = nciVsEl.mean(axis=(1, 2))
nllVsEl = nllVsEl.mean(axis=(1, 2))
# plot influence of changing lengthscale on the RMSE and NCI and NLL filter performance
plt.figure()
plt.semilogx(lscale,
rmseVsEl.squeeze(),
color='k',
ls='-',
lw=2,
marker='o',
label='RMSE')
plt.semilogx(lscale,
nciVsEl.squeeze(),
color='k',
ls='--',
lw=2,
marker='o',
label='NCI')
plt.semilogx(lscale,
nllVsEl.squeeze(),
color='k',
ls='-.',
lw=2,
marker='o',
label='NLL')
plt.grid(True)
plt.legend()
plt.show()
plot_data = {
'el': lscale,
'rmse': rmseVsEl,
'nci': nciVsEl,
'neg_log_likelihood': nllVsEl
}
return plot_data
def tables(steps=500, sims=100):
# setup univariate non-stationary growth model
x0 = GaussRV(1, cov=np.atleast_2d(5.0))
q = GaussRV(1, cov=np.atleast_2d(10.0))
dyn = UNGMTransition(x0, q) # dynamics
r = GaussRV(1)
obs = UNGMMeasurement(r, 1) # observation model
x = dyn.simulate_discrete(steps, mc_sims=sims) # generate some data
z = obs.simulate_measurements(x)
kern_par_sr = np.array([[1.0, 0.3 * dyn.dim_in]])
kern_par_ut = np.array([[1.0, 3.0 * dyn.dim_in]])
kern_par_gh = np.array([[1.0, 0.1 * dyn.dim_in]])
# initialize filters/smoothers
algorithms = (
CubatureKalman(dyn, obs),
UnscentedKalman(dyn, obs),
GaussHermiteKalman(dyn, obs),
GaussHermiteKalman(dyn, obs),
GaussHermiteKalman(dyn, obs),
GaussHermiteKalman(dyn, obs),
GaussHermiteKalman(dyn, obs),
GaussianProcessKalman(dyn, obs, kern_par_sr, kern_par_sr, points='sr'),
GaussianProcessKalman(dyn, obs, kern_par_ut, kern_par_ut, points='ut'),
GaussianProcessKalman(dyn, obs, kern_par_sr, kern_par_sr, points='gh', point_hyp={'degree': 5}),
GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 7}),
GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 10}),
GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 15}),
GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 20}),
)
num_algs = len(algorithms)
# space for estimates
mean_f, cov_f = np.zeros((dyn.dim_in, steps, sims, num_algs)), np.zeros((dyn.dim_in, dyn.dim_in, steps, sims, num_algs))
mean_s, cov_s = np.zeros((dyn.dim_in, steps, sims, num_algs)), np.zeros((dyn.dim_in, dyn.dim_in, steps, sims, num_algs))
# do filtering/smoothing
t0 = time.time() # measure execution time
print('Running filters/smoothers ...', flush=True)
for a, alg in enumerate(algorithms):
for sim in trange(sims, desc='{:25}'.format(alg.__class__.__name__), file=sys.stdout):
mean_f[..., sim, a], cov_f[..., sim, a] = alg.forward_pass(z[..., sim])
mean_s[..., sim, a], cov_s[..., sim, a] = alg.backward_pass()
alg.reset()
print('Done in {0:.4f} [sec]'.format(time.time() - t0))
# evaluate perfomance
scores = evaluate_performance(x, mean_f, cov_f, mean_s, cov_s)
rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s = scores[:6]
rmseStd_f, nciStd_f, nllStd_f, rmseStd_s, nciStd_s, nllStd_s = scores[6:]
# put data into Pandas DataFrame for fancy printing and latex export
row_labels = ['SR', 'UT', 'GH-5', 'GH-7', 'GH-10', 'GH-15',
'GH-20'] # [alg.__class__.__name__ for alg in algorithms]
col_labels = ['Classical', 'Bayesian', 'Classical (2std)', 'Bayesian (2std)']
rmse_table_f = pd.DataFrame(np.hstack((rmseMean_f.reshape(2, 7).T, rmseStd_f.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
nci_table_f = pd.DataFrame(np.hstack((nciMean_f.reshape(2, 7).T, nciStd_f.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
nll_table_f = pd.DataFrame(np.hstack((nllMean_f.reshape(2, 7).T, nllStd_f.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
rmse_table_s = pd.DataFrame(np.hstack((rmseMean_s.reshape(2, 7).T, rmseStd_s.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
nci_table_s = pd.DataFrame(np.hstack((nciMean_s.reshape(2, 7).T, nciStd_s.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
nll_table_s = pd.DataFrame(np.hstack((nllMean_s.reshape(2, 7).T, nllStd_s.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
# print tables
print('Filter RMSE')
print(rmse_table_f)
print('Filter NCI')
print(nci_table_f)
print('Filter NLL')
print(nll_table_f)
print('Smoother RMSE')
print(rmse_table_s)
print('Smoother NCI')
print(nci_table_s)
print('Smoother NLL')
print(nll_table_s)
# return computed metrics for filters and smoothers
return {'filter_RMSE': rmse_table_f, 'filter_NCI': nci_table_f, 'filter_NLL': nll_table_f,
'smoother_RMSE': rmse_table_s, 'smoother_NCI': nci_table_s, 'smoother_NLL': nll_table_s}
def reentry_demo(dur=200, mc_sims=100, outfile=None):
# use default filename if unspecified
if outfile is None:
outfile = 'reentry_demo_results.dat'
outfile = os.path.join('..', outfile)
if not os.path.exists(outfile) or True:
tau = 0.05
disc_tau = 0.1
# initial condition statistics
m0 = np.array([6500, 350, -1.8, -6.8, 0.7])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 0])
init_rv = GaussRV(5, m0, P0)
# process noise statistics
noise_rv = GaussRV(3, cov=np.diag([2.4e-5, 2.4e-5, 0]))
sys = ReentryVehicle2DTransition(init_rv, noise_rv)
# measurement noise statistics
meas_noise_rv = GaussRV(2, cov=np.diag([1e-6, 0.17e-6]))
obs = Radar2DMeasurement(meas_noise_rv,
5,
radar_loc=np.array([6374, 0.0]))
np.random.seed(0)
# Generate reference trajectory by SDE simulation
x = sys.simulate_continuous(duration=dur, dt=tau, mc_sims=mc_sims)
y = obs.simulate_measurements(x)
# subsample data for the filter and performance score calculation
x = x[:, ::2, ...] # take every second point on the trajectory
y = y[:, ::2, ...] # and corresponding measurement
# Initialize state-space model with mis-specified initial mean
# m0 = np.array([6500.4, 349.14, -1.1093, -6.1967, 0.6932])
m0 = np.array([6500, 350, -1.1, -6.1, 0.7])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
init_rv = GaussRV(5, m0, P0)
noise_rv = GaussRV(3, cov=np.diag([2.4e-5, 2.4e-5, 1e-6]))
dyn = ReentryVehicle2DTransition(init_rv, noise_rv, dt=disc_tau)
# NOTE: higher tau causes higher spread of trajectories at the ends
# Initialize filters
par_dyn = np.array([[1.0, 1, 1, 1, 1, 1]])
par_obs = np.array([[1.0, 0.9, 0.9, 1e4, 1e4,
1e4]]) # np.atleast_2d(np.ones(6))
mul_ut = np.hstack((np.zeros((dyn.dim_in, 1)), np.eye(dyn.dim_in),
2 * np.eye(dyn.dim_in))).astype(np.int)
alg = OrderedDict({
# GaussianProcessKalman(ssm, par_dyn, par_obs, kernel='rbf', points='ut'),
'bsqkf':
BayesSardKalman(dyn,
obs,
par_dyn,
par_obs,
mul_ut,
mul_ut,
points='ut'),
'bsqkf_2e-6':
BayesSardKalman(dyn,
obs,
par_dyn,
par_obs,
mul_ut,
mul_ut,
points='ut'),
'bsqkf_2e-7':
BayesSardKalman(dyn,
obs,
par_dyn,
par_obs,
mul_ut,
mul_ut,
points='ut'),
'ukf':
UnscentedKalman(dyn, obs, beta=0.0),
})
alg['bsqkf'].tf_dyn.model.model_var = np.diag(
[0.0002, 0.0002, 0.0002, 0.0002, 0.0002])
alg['bsqkf'].tf_obs.model.model_var = 0 * np.eye(2)
alg['bsqkf_2e-6'].tf_dyn.model.model_var = 2e-6 * np.eye(5)
alg['bsqkf_2e-6'].tf_obs.model.model_var = 0 * np.eye(2)
alg['bsqkf_2e-7'].tf_dyn.model.model_var = 2e-7 * np.eye(5)
alg['bsqkf_2e-7'].tf_obs.model.model_var = 0 * np.eye(2)
# print experiment setup
print('System (reality)')
print('{:10s}: {}'.format('x0_mean', sys.init_rv.mean))
print('{:10s}: {}'.format('x0_cov', sys.init_rv.cov.diagonal()))
print()
print('State-Space Model')
print('{:10s}: {}'.format('x0_mean', dyn.init_rv.mean))
print('{:10s}: {}'.format('x0_cov', dyn.init_rv.cov.diagonal()))
print()
print('BSQ Expected Model Variance')
print('{:10s}: {}'.format(
'dyn_func', alg['bsqkf'].tf_dyn.model.model_var.diagonal()))
print('{:10s}: {}'.format(
'obs_func', alg['bsqkf'].tf_obs.model.model_var.diagonal()))
print()
num_alg = len(alg)
d, steps, mc_sims = x.shape
mean, cov = np.zeros((d, steps, mc_sims, num_alg)), np.zeros(
(d, d, steps, mc_sims, num_alg))
for ia, a in enumerate(alg):
print('Running {:<5} ... '.format(a.upper()), end='', flush=True)
t0 = time.time()
for imc in range(mc_sims):
mean[..., imc, ia], cov[..., imc,
ia] = alg[a].forward_pass(y[..., imc])
alg[a].reset()
print('{:>30}'.format('Done in {:.2f} [sec]'.format(time.time() -
t0)))
# Performance score plots
print()
print('Computing performance scores ...')
error2 = mean.copy()
lcr_x = np.zeros((steps, mc_sims, num_alg))
lcr_pos = lcr_x.copy()
lcr_vel = lcr_x.copy()
lcr_theta = lcr_x.copy()
for a in range(num_alg):
for k in range(steps):
mse = mse_matrix(x[:, k, :], mean[:, k, :, a])
for imc in range(mc_sims):
error2[:, k, imc,
a] = squared_error(x[:, k, imc], mean[:, k, imc, a])
lcr_x[k, imc,
a] = log_cred_ratio(x[:, k, imc], mean[:, k, imc, a],
cov[:, :, k, imc, a], mse)
lcr_pos[k, imc,
a] = log_cred_ratio(x[:2, k, imc], mean[:2, k, imc,
a],
cov[:2, :2, k, imc,
a], mse[:2, :2])
lcr_vel[k, imc,
a] = log_cred_ratio(x[2:4, k, imc], mean[2:4, k,
imc, a],
cov[2:4, 2:4, k, imc,
a], mse[2:4, 2:4])
lcr_theta[k, imc,
a] = log_cred_ratio(x[4, k, imc], mean[4, k, imc,
a],
cov[4, 4, k, imc, a], mse[4,
4])
# Averaged RMSE and Inclination Indicator in time
x_rmse_vs_time = np.sqrt(error2.sum(axis=0)).mean(axis=1)
pos_rmse_vs_time = np.sqrt((error2[:2, ...]).sum(axis=0)).mean(axis=1)
vel_rmse_vs_time = np.sqrt((error2[2:4, ...]).sum(axis=0)).mean(axis=1)
theta_rmse_vs_time = np.sqrt((error2[4, ...])).mean(axis=1)
x_inc_vs_time = lcr_x.mean(axis=1)
pos_inc_vs_time = lcr_pos.mean(axis=1)
vel_inc_vs_time = lcr_vel.mean(axis=1)
theta_inc_vs_time = lcr_theta.mean(axis=1)
print('{:10s}: {}'.format('RMSE', x_rmse_vs_time.mean(axis=0)))
print('{:10s}: {}'.format('INC', x_inc_vs_time.mean(axis=0)))
# Save the simulation results into outfile
data_dict = {
'duration': dur,
'disc_tau': disc_tau,
'alg_str': list(alg.keys()),
'x': x,
'mean': mean,
'cov': cov,
'state': {
'rmse': x_rmse_vs_time,
'inc': x_inc_vs_time
},
'position': {
'rmse': pos_rmse_vs_time,
'inc': pos_inc_vs_time
},
'velocity': {
'rmse': vel_rmse_vs_time,
'inc': vel_inc_vs_time
},
'parameter': {
'rmse': theta_rmse_vs_time,
'inc': theta_inc_vs_time
},
}
joblib.dump(data_dict, outfile)
# Visualize simulation results, plots, tables
reentry_demo_results(data_dict)
else:
reentry_demo_results(joblib.load(outfile))
def reentry_analysis_demo(dur=50, mc_sims=10):
tau = 0.05
disc_tau = 0.1
# initial condition statistics
m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 0])
init_rv = GaussRV(5, m0, P0)
# process noise statistics
noise_rv = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 0]))
sys = ReentryVehicle2DTransition(init_rv, noise_rv)
# measurement noise statistics
dim_obs = 2
meas_noise_rv = GaussRV(dim_obs, cov=np.diag([1e-6, 0.17e-6]))
obs = Radar2DMeasurement(meas_noise_rv, 5, radar_loc=np.array([6374, 0.0]))
np.random.seed(0)
# Generate reference trajectory by SDE simulation
x = sys.simulate_continuous(duration=dur, dt=tau, mc_sims=mc_sims)
y = obs.simulate_measurements(x)
# subsample data for the filter and performance score calculation
x = x[:, ::2, ...] # take every second point on the trajectory
y = y[:, ::2, ...] # and corresponding measurement
# Initialize state-space model with mis-specified initial mean
m0 = np.array([6500.4, 349.14, -1.1093, -6.1967, 0.6932])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
init_rv = GaussRV(5, m0, P0)
noise_rv = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 1e-6]))
dyn = ReentryVehicle2DTransition(init_rv, noise_rv, dt=disc_tau)
# NOTE: higher tau causes higher spread of trajectories at the ends
# Initialize filters
par_dyn = np.array([[1.0, 1, 1, 1, 1, 1]])
par_obs = np.array([[1.0, 0.9, 0.9, 1e4, 1e4,
1e4]]) # np.atleast_2d(np.ones(6))
mul_ut = np.hstack((np.zeros((dyn.dim_in, 1)), np.eye(dyn.dim_in),
2 * np.eye(dyn.dim_in))).astype(np.int)
alg = OrderedDict({
# GaussianProcessKalman(ssm, par_dyn, par_obs, kernel='rbf', points='ut'),
'bsqkf':
BayesSardKalman(dyn,
obs,
par_dyn,
par_obs,
mul_ut,
mul_ut,
points='ut'),
'ukf':
UnscentedKalman(dyn, obs, kappa=0.0, beta=2.0),
})
alg['bsqkf'].tf_dyn.model.model_var = np.diag([0.1, 0.1, 0.1, 0.1, 0.015])
alg['bsqkf'].tf_obs.model.model_var = 0 * np.eye(2)
print('BSQ EMV\ndyn: {} \nobs: {}'.format(
alg['bsqkf'].tf_dyn.model.model_var.diagonal(),
alg['bsqkf'].tf_obs.model.model_var.diagonal()))
# FILTERING
num_alg = len(alg)
d, steps, mc_sims = x.shape
mean, cov = np.zeros((d, steps, mc_sims, num_alg)), np.zeros(
(d, d, steps, mc_sims, num_alg))
mean_x, cov_x = mean.copy(), cov.copy()
mean_y, cov_y = np.zeros((dim_obs, steps, mc_sims, num_alg)), np.zeros(
(dim_obs, dim_obs, steps, mc_sims, num_alg))
cov_yx = np.zeros((dim_obs, d, steps, mc_sims, num_alg))
# y = np.hstack((np.zeros((dim_obs, 1, mc_sims)), y))
for ia, a in enumerate(alg):
print('Running {:<5} ... '.format(a.upper()), end='', flush=True)
t0 = time.time()
for imc in range(mc_sims):
for k in range(steps):
# TIME UPDATE
alg[a]._time_update(k)
# record predictive moments
mean_x[:, k, imc,
ia], cov_x[..., k, imc,
ia] = alg[a].x_mean_pr, alg[a].x_cov_pr
mean_y[:, k, imc,
ia], cov_y[..., k, imc,
ia] = alg[a].y_mean_pr, alg[a].y_cov_pr
cov_yx[..., k, imc, ia] = alg[a].xy_cov
# MEASUREMENT UPDATE
alg[a]._measurement_update(y[:, k, imc])
# record filtered moments
mean[:, k, imc,
ia], cov[..., k, imc,
ia] = alg[a].x_mean_fi, alg[a].x_cov_fi
# reset filter storage
alg[a].reset()
print('{:>30}'.format('Done in {:.2f} [sec]'.format(time.time() - t0)))
