Week 10 - Filtering and State Estimation¶
In [2]:
import numpy as np
import matplotlib.pyplot as plt
In the limit of small measurement noise we get:
$$ \begin{split} N_t^y &\rightarrow 0 \\ B_t E_{t|t-1} B_t^T + N_t^y &\approx B_t E_{t|t-1} B_t^T \end{split} $$$$ K_t = E_{t|t-1} B_t^T (B_t E_{t|t-1} B_t^T + N_t^y)^{-1} \approx E_{t|t-1} B_t^T (B_t E_{t|t-1} B_t^T)^{-1} = B_t^{-1} $$And for the error matrix we get:
$$ E_{t|t} = (I-K_t B_t) E_{t|t-1} \approx (I-B_t^{-1} B_t) E_{t|t-1} = 0 $$Problem 19.2:¶
Take as a test signal a periodically modulated sinusoid with noise added, $$y_n = sin[0.1t_n + 4 sin(0.01t_n)] + η ≡ sin(θ_n) + η , $$ where $η$ is a Gaussian noise process with $σ = 0.1$. Design an extended Kalman filter to estimate the noise-free signal. Use a two-component state vector $\vec{x_n} = (θ_n, θ_{n−1})$, and assume for the internal model a linear extrapolation $θ_{n+1} = θ_n + (θ_n−θ_{n−1})$. Take the system noise matrix $N^x$ to be diagonal, and plot the predicted value of $y$ versus the measured value of $y$ if the standard deviation of the system noise is chosen to be $10^{−1}$, $10^{−3}$, and $10^{−5}$. Use the identity matrix for the initial error estimate.¶
In [59]:
delta_t = .1
T = np.arange(0, 1000, delta_t)
theta = .1*T + 4*np.sin(.01*T)
eta = np.random.normal(loc=0, scale=.1, size=T.size)
Y = np.sin(theta) + eta
In [60]:
def ExtendedKalmanStep(A_func, B_func, f, g, x_est, y, E_est, N_x, N_y):
# Estimate the new observable
y_est = g(x_est)
# Compute the Kalman gain matrix
K = np.matmul(E_est,
np.matmul(np.transpose(B_func(x_est)),
np.linalg.inv(np.matmul(B_func(x_est),
np.matmul(E_est, np.transpose(B_func(x_est)))) + N_y)))
# Estimate the new state
x = x_est + np.matmul(K, y - y_est)
# Update the error matrix
E = np.matmul(np.eye(N=K.shape[0], M=B_func(x_est).shape[1]) - np.matmul(K, B_func(x_est)), E_est)
# Predict the new state
x_est = f(x)
# Predict the new error
E_est = np.matmul(A_func(x), np.matmul(E, np.transpose(A_func(x)))) + N_x
return x_est, y_est, E_est
def KalmanFilter(A_func, B_func, f, g, init_state, init_obs, Y, init_E, N_x, N_y):
x_est = init_state
E_est = init_E
Y_est = [init_obs]
X_est = [x_est]
for y in Y[1:]:
x_est, y_est, E_est = ExtendedKalmanStep(A_func, B_func, f, g, x_est, y, E_est, N_x, N_y)
Y_est.append(y_est)
X_est.append(x_est)
return X_est, Y_est
In [61]:
def main(Y, std):
A_func = lambda x: np.array([[2, -1], [1, 0]])
B_func = lambda x: np.array([[np.cos(x[0]), 0], [0, np.cos(x[1])]])
f = lambda x: (2*x[0]-x[1], x[0])
g = lambda x: (np.sin(x[0]), np.sin(x[1]))
init_state = np.array([0, 0])
init_obs = (Y[0], Y[0])
init_E = np.eye(2, 2)
N_x = np.diag([std**2, std**2])
N_y = np.diag([.01, .01])
return KalmanFilter(A_func, B_func, f, g, init_state, init_obs, Y, init_E, N_x, N_y)
In [62]:
X_est, Y_est = main(Y, 1e-1)
Y_est = [y[0] for y in Y_est]
plt.plot(T, Y, 'b', label='True observations')
plt.plot(T, Y_est, 'r', label='Predicted observations', alpha=.8)
plt.xlabel('T')
plt.ylabel('Value')
plt.title('Standard deviation 1e-1')
plt.legend()
plt.show()
In [63]:
X_est, Y_est = main(Y, 1e-3)
Y_est = [y[0] for y in Y_est]
plt.plot(T, Y, 'b', label='True observations')
plt.plot(T, Y_est, 'r', label='Predicted observations')
plt.xlabel('T')
plt.ylabel('Value')
plt.title('Standard deviation 1e-3')
plt.legend()
plt.show()
In [68]:
X_est, Y_est = main(Y, 1e-7)
Y_est = [y[0] for y in Y_est]
plt.plot(T, Y, 'b', label='True observations')
plt.plot(T, Y_est, 'r', label='Predicted observations')
plt.xlabel('T')
plt.ylabel('Value')
plt.title('Standard deviation 1e-7')
plt.legend()
plt.show()
Problem 19.3:¶
(a) Given an HMM trellis (Figure 19.5), work out the probability to see an internal state given the observations $p(x_t|y_1, \dots , y_T)$ in terms of forward and backward terms.¶
$$ \begin{split}
p(x_t|y_1,\dots ,y_T) &= \frac{p(x_t,y_1,\dots ,y_T)}{p(y_1,\dots ,y_T)} \\
\end{split} $$$$ \begin{split}
p(x_t,y_1,\dots ,y_T) =& \sum_{x_T=1}^N p(y_T|x_T)p(x_T,x_t,y_1,\dots ,y_{T-1}) \\
=&\sum_{x_T=1}^N p(y_T|x_T)\sum_{x_{T-1}=1}^N p(y_{T-1}|x_{T-1})p(x_T|x_{T-1})p(x_{T-1},x_t,y_1,\dots ,y_{T-2}) \\
=&\sum_{x_T=1}^N p(y_T|x_T)\sum_{x_{T-1}=1}^N p(y_{T-1}|x_{T-1})p(x_T|x_{T-1}) \dots \sum_{x_{t+1}=1}^N p(y_{t+1}|x_{t+1})p(x_{t+2}|x_{t+1})p(x_{t+1},x_t,y_1,\dots ,y_{t}) \\
=&\sum_{x_T=1}^N p(y_T|x_T)\sum_{x_{T-1}=1}^N p(y_{T-1}|x_{T-1})p(x_T|x_{T-1}) \dots \\
&\sum_{x_{t+1}=1}^N p(y_{t+1}|x_{t+1}) p(x_{t+2}|x_{t+1}) p(y_t|x_t) p(x_{t+1}|x_t) p(x_t,y_1,\dots ,y_{t-1}) \\
=&\sum_{x_T=1}^N p(y_T|x_T)\sum_{x_{T-1}=1}^N p(y_{T-1}|x_{T-1})p(x_T|x_{T-1}) \dots \\
&\sum_{x_{t+1}=1}^N p(y_{t+1}|x_{t+1}) p(x_{t+2}|x_{t+1}) p(y_t|x_t) p(x_{t+1}|x_t) \sum_{x_{t-1}=1}^N p(y_{t-1}|x_{t-1}) p(x_t|x_{t-1}) \dots \\
& \sum_{x_{1}=1}^N p(y_{1}|x_{1}) p(x_2|x_1) p(x_1) \\
\\
p(y_1,\dots ,y_T) =& \sum_{x_T=1}^N p(y_T|x_T)\sum_{x_{T-1}=1}^N p(y_{T-1}|x_{T-1})p(x_T|x_{T-1}) \dots \\
&\sum_{x_{2}=1}^N p(y_{2}|x_{2}) p(x_3|x_{2}) \sum_{x_{1}=1}^N p(y_{1}|x_{1}) p(x_2|x_1) p(x_1)
\end{split} $$
(b) Generate observations from the model in Figure 19.4, with a fair and a biased coin as the internal states, and probabilities to switch between them.¶
In [21]:
probs = np.array([[.5, .5], [.25, .75]])
trans_p = np.array([[.99, .01], [.01, .99]])
T = 900
coins = [np.random.choice([0, 1])]
obs = [np.random.choice([0, 1], p=probs[coins[-1]])]
for _ in range(T-1):
coins.append(np.random.choice([0, 1], p=trans_p[coins[-1]]))
obs.append(np.random.choice([0, 1], p=probs[coins[-1]]))
(c) Use knowledge of this model and those observations to estimate the probabilities for which coin was used when, and compare with the correct values.¶
In [22]:
def forward_rec(obs, trans_probs, obs_probs, start_probs):
"""Computes p(x_t, y_1, ... , y_{t-1})."""
"""Returns vector"""
if len(obs) == 0:
return start_probs
state_obs_probs = forward_rec(obs[:-1], trans_probs, obs_probs, start_probs)*obs_probs[:,obs[-1]]
return np.matmul(state_obs_probs, trans_probs)
def forward(obs, trans_probs, obs_probs, start_probs):
"""Computes p(y_1, ... , y_T)"""
"""Returns scalar"""
state_obs_probs = forward_rec(obs[:-1], trans_probs, obs_probs, start_probs)*obs_probs[:,obs[-1]]
return np.sum(state_obs_probs)
def backward(obs, trans_probs, obs_probs, flag=True):
"""Computes p(y_t, ... , y_T | x_t)"""
"""Returns vector"""
if flag:
state_obs_probs = obs_probs[:,obs[0]]*backward(obs[1:], trans_probs, obs_probs, flag=False)
return state_obs_probs
if len(obs) == 1:
return np.matmul(trans_probs, obs_probs[:,obs[0]])
else:
try:
state_obs_probs = obs_probs[:,obs[0]]*backward(obs[1:], trans_probs, obs_probs, flag=False)
return np.matmul(trans_probs, state_obs_probs)
except:
print(obs)
In [23]:
obs_likelihood = forward(obs, trans_p, probs, [.5, .5])
backward_probs = np.array([backward(obs[t:], trans_p, probs) for t in range(T-1)])
forward_probs = np.array([forward_rec(obs[:t], trans_p, probs, [.5, .5]) for t in range(T-1)])
In [24]:
coin_probs = backward_probs*forward_probs/obs_likelihood
coin_probs = np.concatenate(([[.5, .5]], coin_probs))
In [25]:
accuracy = np.mean(np.equal(np.argmax(coin_probs, axis=-1), coins))
print('Accuracy = %f' % accuracy)
In [ ]: