Radonov, Ivan, 5967988

This cell contains LaTeX macros +++

+++++++++++++++++++++++++++

Build the state-space model

for states and measurements

uk D
^k
u d
xk = ( ) ∈ R yk = ( ) ∈ R k = 1, … , T
vk ^k
v

In [1]:
import numpy as np

from probnum import randvars

from scipy.linalg import cho_factor, cho_solve

from matplotlib import pyplot as plt

from tueplots import bundles

plt.rcParams.update(bundles.beamer_moml())

plt.rcParams.update({"figure.dpi":200})

rng = np.random.default_rng(seed=124)

T = 200

Initial distribution
x0 ∼ N (μ0 , Σ0 )

with

⊤
μ0 = [−1  1] (1)

1.0 0.0
Σ0 = ( ) (2)
0.0 1.0

In [2]:
mu_0 = np.array([-1, 1])

Sigma_0 = np.eye(2)

Dynamics
xk ∣ xk−1 ∼ N (f(xk−1 ), Qk−1 )

with

3
uk u k + 0.1 ⋅ (c ⋅ (u k − u /3 + vk ))
k
f(xk ) = f (( )) = ( ) (3)
vk vk + 0.1 ⋅ (−(1/c) ⋅ (u k − a − b ⋅ vk ))

0.01 0.0
Qk = Q = ( ) (4)
0.0 0.01

and

a = 0.2 (5)

b = 0.2 (6)

c = 3.0 (7)

In [3]:
a, b, c = .2, .2, 3.

Q = 0.01 * np.eye(2)

def f(x):

u, v = x

return np.array([u + .1*c*(u - 1/3*u**3 + v), v - .1/c * (u - a - b * v)])

def Jf(x):

u, v = x

return np.array([[1 + 0.1*c*(1 - u**2), -0.1/c], [0.1*c, 1 + 0.1*b/c]])

Measurement model
yk ∣ xk ∼ N (h(xk ), R k )

with

h(xk ) = xk (8)

1 0
Rk = ( ) (9)
0 1

In [4]:
h = lambda x: x

def Jh(x):

n = x.shape[0]

return np.ones((n, n))

Rk = np.eye(2)

Simulate a trajectory and draw noisy observations

In [5]:
def simulate():

ground_truth = []

path = []

observations = []

gk = mu_0

xk = randvars.Normal(mean=mu_0, cov=Sigma_0).sample(rng=rng)

for k in range(T):

gk = f(gk)

ground_truth.append(gk)

xk = randvars.Normal(mean=f(xk), cov=Q).sample(rng=rng)

path.append(xk)

yk = randvars.Normal(mean=h(xk), cov=Rk).sample(rng=rng)

observations.append(yk)

return ground_truth, path, observations

In [6]:
ground_truth, path, observations = simulate()

Plot groundtruth and data

In [8]:
fig, ax = plt.subplots(1,2, sharey=True)

ax[0].set_title('Ground truth')

ax[0].plot(ground_truth)

ax[1].set_title('Data')

ax[1].plot(observations, alpha=0.3, label='Observations')

ax[1].plot(path, label='Latent state')

fig.legend()

<matplotlib.legend.Legend at 0x1635667df28>
Out[8]:

Filtering — The extended Kalman Filter

Prediction
−
μ = f(μk−1 ) (10)

− ⊤
Σ = J f (μk−1 )Σk−1 J f (μk−1 ) + Qk−1 (11)

In [9]:
def ekf_predict(mu_prev, Sigma_prev):

mu_minus = f(mu_prev)

J_prev = Jf(mu_prev)

Sigma_minus = J_prev @ Sigma_prev @ J_prev.T + Q

return mu_minus, Sigma_minus

Correction
−
^ = h(μ
y ) (12)
k k

− − − ⊤
S k = J h (μ )Σ J h (μ ) + Rk (13)
k k k

− − ⊤ −1
Kk = Σ J h (μ ) S (14)
k k k

−
μk = μ ^ )
+ Kk (yk − y (15)
k k

− ⊤
Σk = Σ − Kk S k K (16)
k k

In [10]:
def compute_S_inv(J_minus, Sigma_minus, Rk):

# use the matrix inversion lemma

R_inv = Rk

b = Sigma_minus @ J_minus.T @ R_inv

A = np.eye(J_minus.shape[0]) + b @ J_minus

return R_inv - R_inv @ J_minus @ cho_solve(cho_factor(A), b)

def ekf_correct(mu_minus, Sigma_minus, y):

yhat = h(mu_minus)

J_minus = Jh(mu_minus)

Sk = J_minus @ Sigma_minus @ J_minus.T + Rk

Sk_inv = compute_S_inv(J_minus, Sigma_minus, Rk)

Kk = Sigma_minus @ J_minus.T @ Sk_inv

mu_k = mu_minus + Kk @ (y - yhat)

Sigma_k = Sigma_minus - Kk @ Sk @ Kk.T

return mu_k, Sigma_k

Compute filtering estimate

In [11]:
mu_k, Sigma_k = mu_0, Sigma_0

prediction_means = []

prediction_covs = []

filtered_means = []

filtered_covs = []

for k in range(T):

mu_minus, Sigma_minus = ekf_predict(mu_k, Sigma_k)

prediction_means.append(mu_minus)

prediction_covs.append(Sigma_minus)

mu_k, Sigma_k = ekf_correct(mu_minus, Sigma_minus, observations[k])

filtered_means.append(mu_k)

filtered_covs.append(Sigma_k)

Plot filtering estimate

In [12]:
fig, ax = plt.subplots(1,2, sharey=True)

ax[0].set_title('Data')

ax[0].plot(observations, alpha=0.3, label='Observations')

ax[0].plot(path, label='Latent state')

ax[1].set_title('Filtering estimate')

ax[1].plot(filtered_means)

fig.legend()

<matplotlib.legend.Legend at 0x163567bcb38>
Out[12]:

Smoothing — The Rauch-Tung-Striebel Smoother

−1
⊤ −
Gk = Σk J f (μk ) [Σ ] (17)
k+1

−
ξk = μk + Gk (ξk+1 − μ ) (18)
k+1

− ⊤
Λk = Σk + Gk (Λk+1 − Σ )G (19)
k+1 k

In [13]:
def compute_Gk(Sigma_k, mu_k, Sigma_minus_next):

return Sigma_k @ cho_solve(cho_factor(Sigma_minus_next), Jf(mu_k)).T

def eks_step(k, e_next, L_next, mu_k, mu_minus_next, Sigma_k, Sigma_minus_next):

G_k = compute_Gk(Sigma_k, mu_k, Sigma_minus_next)

ek = mu_k + G_k @ (e_next - mu_minus_next)

Lk = Sigma_k + G_k @ (L_next - Sigma_minus_next) @ G_k.T

return ek, Lk

Compute the smoothing posterior

In [14]:
e_next, L_next = filtered_means[T-1], filtered_covs[T-1]

smoothed_means = [e_next]

smoothed_covs = [L_next]

for k in range(T-2, -1, -1):

e_next, L_next = eks_step(k, e_next, L_next, filtered_means[k], prediction_means[k+1], filtered_covs[k], prediction_covs[k+1])

smoothed_means.append(e_next)

smoothed_covs.append(L_next)

# reverse, since we started from time T

smoothed_means = smoothed_means[::-1]

smoothed_covs = smoothed_covs[::-1]

Plot the smoothing posterior

In [15]:
plt.title('Smoothing posterior')

plt.plot(smoothed_means)

[<matplotlib.lines.Line2D at 0x163584beda0>,

Out[15]:
<matplotlib.lines.Line2D at 0x16356b159b0>]

In [16]:
fig, axes = plt.subplots(1, 2, sharey=True)

time_grid = np.arange(200)

gt_u = [x[0] for x in ground_truth]

sp_u = [x[0] for x in path]

ob_u = [x[0] for x in observations]

sm_u = [x[0] for x in smoothed_means]

gt_v = [x[1] for x in ground_truth]

sp_v = [x[1] for x in path]

ob_v = [x[1] for x in observations]

sm_v = [x[1] for x in smoothed_means]

axes[0].set_title('u')

axes[0].plot(time_grid, gt_u, label='Ground truth', alpha=.7)

axes[0].plot(time_grid, sp_u, label='Latent states', alpha=.4)

axes[0].scatter(time_grid, ob_u, label='Observations', alpha=.4, s=1)

axes[0].plot(time_grid, sm_u, label='Smoothed posterior')

axes[1].set_title('v')

axes[1].plot(time_grid, gt_v, alpha=.7)

axes[1].plot(time_grid, sp_v, alpha=.4)

axes[1].scatter(time_grid, ob_v, alpha=.4, s=1)

axes[1].plot(time_grid, sm_v)

std_u = np.sqrt([smoothed_covs[i][0, 0] for i in range(T)])

std_v = np.sqrt([smoothed_covs[i][1, 1] for i in range(T)])

axes[0].fill_between(

time_grid,

sm_u - 1.96 * std_u,

sm_u + 1.96 * std_u,

alpha=0.2,

label="1.96 marginal stddev",

axes[1].fill_between(

time_grid, sm_v - 1.96 * std_v, sm_v + 1.96 * std_v, alpha=0.2

fig.legend()

<matplotlib.legend.Legend at 0x16356c23320>
Out[16]: