Kalman Filter



In [2]:

    
from sympy import *
import utils as utils
import SuperMatExpr as SME
import MVG as MVG
from IPython.display import display, Math, Latex



In [3]:

    
# Shapes
m, n = symbols('m n')

1. Variables



In [7]:

    
# Variables                               
x_prev = SME.SuperMatSymbol(m,1,name='x_{t-1}',mat_type='var')    # x_(t-1)
x_t = SME.SuperMatSymbol(m,1,name='x_t',mat_type='var')           # x_t
y_prev = SME.SuperMatSymbol(n,1,name='y_{1:t-1}',mat_type='var')  # y_(1:t-1) 
y_t = SME.SuperMatSymbol(n,1,name='y_t',mat_type='var')           # y_t
b_t = SME.SuperMatSymbol(m,1,name='b_t',mat_type='var')           # b_t
d_t = SME.SuperMatSymbol(n,1,name='d_t',mat_type='var')           # d_t
w_t = SME.SuperMatSymbol(m,1,name='w_t',mat_type='var')           # w_t
e_t = SME.SuperMatSymbol(n,1,name='e_t',mat_type='var')           # e_t

2. Constant Parameters



In [8]:

    
# Constant parameters
A = SME.SuperMatSymbol(m,m,name='A',mat_type='other')                                     
Q = SME.SuperMatSymbol(m,m,name='Q',mat_type='covar',dep_vars=[w_t])
R = SME.SuperMatSymbol(n,n,name='R',mat_type='covar',dep_vars=[e_t])
C = SME.SuperMatSymbol(n,m,name='B',mat_type='other')

3. p(x(t-1)|y(1:t-1))



In [9]:

    
# p(x_(t-1)|y_(1:t-1))
f_prev = SME.SuperMatSymbol(m,1,name='m_{t|t}',mat_type='mean',dep_vars=[x_prev])   # \hat{x}_(t|t) (mean)
F_prev = SME.SuperMatSymbol(m,m,name='P_{t|t}',mat_type='covar',dep_vars=[x_prev])  # P_(t|t) (covariance)
p_xprev = MVG.MVG([x_prev], moments=[f_prev, F_prev, utils.getZ(F_prev)], conditioned_vars=[y_prev])

print("p_xprev:")
display(Latex(utils.matLatex(p_xprev)))









    



p_xprev:






    





\begin{align*}
p\left(\mathbf{x_{t-1}}|\mathbf{y_{1:t-1}}\right)&= \mathcal{N}\left(\mathbf{m}_{\mathbf{x_{t-1}}|\mathbf{y_{1:t-1}}},\mathbf{\Sigma}_{\mathbf{x_{t-1}}|\mathbf{y_{1:t-1}}}\right)\\
\end{align*}

4. p(xt|x(t-1))



In [10]:

    
m_xt = SME.SuperMatSymbol(m,1,mat_type='mean',dep_vars=[x_t],cond_vars=[x_prev],expanded=A*x_prev + b_t) 
cov_xt = SME.SuperMatSymbol(m,m,mat_type='covar',dep_vars=[x_t],cond_vars=[x_prev],expanded=Q)
p_xt = MVG.MVG([x_t],moments=[m_xt, cov_xt, utils.getZ(cov_xt)], conditioned_vars=[x_prev])

print("p_xt:")
display(Latex(utils.matLatex(p_xt)))









    



p_xt:






    





\begin{align*}
p\left(\mathbf{x_t}|\mathbf{x_{t-1}}\right)&= \mathcal{N}\left(\mathbf{m}_{\mathbf{x_t}|\mathbf{x_{t-1}}},\mathbf{\Sigma}_{\mathbf{x_t}|\mathbf{x_{t-1}}}\right)\\
\mathbf{m}_{\mathbf{x_t}|\mathbf{x_{t-1}}} &= \mathbf{A} \mathbf{x_{t-1}} + \mathbf{b_t}\\
\mathbf{\Sigma}_{\mathbf{x_t}|\mathbf{x_{t-1}}} &= \Sigma_{\mathbf{w_t}}\\
\end{align*}

5. p(xt,x(t-1)|y_(1:t-1))



In [11]:

    
p_xt_xprev = p_xt*p_xprev

print("p_xt_xprev:")
display(Latex(utils.matLatex(p_xt_xprev)))









    



p_xt_xprev:






    





\begin{align*}
p\left(\mathbf{x_t},\mathbf{x_{t-1}}|\mathbf{y_{1:t-1}}\right)&= \mathcal{N}\left(\mathbf{m}_{\mathbf{x_t},\mathbf{x_{t-1}}|\mathbf{y_{1:t-1}}},\mathbf{\Sigma}_{\mathbf{x_t},\mathbf{x_{t-1}}|\mathbf{y_{1:t-1}}}\right)\\
\mathbf{m}_{\mathbf{x_t},\mathbf{x_{t-1}}|\mathbf{y_{1:t-1}}} &= \left[\begin{smallmatrix}\mathbf{A} \mu_{\mathbf{x_{t-1}}} + \mathbf{b_t}\\\mu_{\mathbf{x_{t-1}}}\end{smallmatrix}\right]\\
\mathbf{\Sigma}_{\mathbf{x_t},\mathbf{x_{t-1}}|\mathbf{y_{1:t-1}}} &= \left[\begin{smallmatrix}\mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}&\mathbf{A} \Sigma_{\mathbf{x_{t-1}}}\\\Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T&\Sigma_{\mathbf{x_{t-1}}}\end{smallmatrix}\right]\\
\end{align*}

6. p(xt|y(1:t-1))



In [12]:

    
p_xt_predict = p_xt_xprev.marginalize([x_prev])

print("p_xt_predict:")
display(Latex(utils.matLatex(p_xt_predict)))









    



p_xt_predict:






    





\begin{align*}
p\left(\mathbf{x_t}|\mathbf{y_{1:t-1}}\right)&= \mathcal{N}\left(\mathbf{m}_{\mathbf{x_t}|\mathbf{y_{1:t-1}}},\mathbf{\Sigma}_{\mathbf{x_t}|\mathbf{y_{1:t-1}}}\right)\\
\mathbf{m}_{\mathbf{x_t}|\mathbf{y_{1:t-1}}} &= \mathbf{A} \mu_{\mathbf{x_{t-1}}} + \mathbf{b_t}\\
\mathbf{\Sigma}_{\mathbf{x_t}|\mathbf{y_{1:t-1}}} &= \mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}\\
\end{align*}

7. p(y_t|x_t)



In [14]:

    
m_yt = SME.SuperMatSymbol(n,1,mat_type='mean',dep_vars=[y_t],cond_vars=[x_t],expanded=C*x_t + d_t)
cov_yt = SME.SuperMatSymbol(n,n,mat_type='covar',dep_vars=[y_t],cond_vars=[x_t],expanded=R)
p_yt = MVG.MVG([y_t], moments=[m_yt,cov_yt,utils.getZ(cov_yt)],conditioned_vars=[x_t])

print("p_yt:")
display(Latex(utils.matLatex(p_yt)))









    



p_yt:






    





\begin{align*}
p\left(\mathbf{y_t}|\mathbf{x_t}\right)&= \mathcal{N}\left(\mathbf{m}_{\mathbf{y_t}|\mathbf{x_t}},\mathbf{\Sigma}_{\mathbf{y_t}|\mathbf{x_t}}\right)\\
\mathbf{m}_{\mathbf{y_t}|\mathbf{x_t}} &= \mathbf{B} \mathbf{x_t} + \mathbf{d_t}\\
\mathbf{\Sigma}_{\mathbf{y_t}|\mathbf{x_t}} &= \Sigma_{\mathbf{e_t}}\\
\end{align*}

8. p(y_t,xt|y(1:t-1))



In [15]:

    
p_yt_xt = p_yt*p_xt_predict

print("p_yt_xt:")
display(Latex(utils.matLatex(p_yt_xt)))









    



p_yt_xt:






    





\begin{align*}
p\left(\mathbf{y_t},\mathbf{x_t}|\mathbf{y_{1:t-1}}\right)&= \mathcal{N}\left(\mathbf{m}_{\mathbf{y_t},\mathbf{x_t}|\mathbf{y_{1:t-1}}},\mathbf{\Sigma}_{\mathbf{y_t},\mathbf{x_t}|\mathbf{y_{1:t-1}}}\right)\\
\mathbf{m}_{\mathbf{y_t},\mathbf{x_t}|\mathbf{y_{1:t-1}}} &= \left[\begin{smallmatrix}\mathbf{B} \left(\mathbf{A} \mu_{\mathbf{x_{t-1}}} + \mathbf{b_t}\right) + \mathbf{d_t}\\\mathbf{A} \mu_{\mathbf{x_{t-1}}} + \mathbf{b_t}\end{smallmatrix}\right]\\
\mathbf{\Sigma}_{\mathbf{y_t},\mathbf{x_t}|\mathbf{y_{1:t-1}}} &= \left[\begin{smallmatrix}\mathbf{B} \left(\mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}\right) \mathbf{B}^T + \Sigma_{\mathbf{e_t}}&\mathbf{B} \left(\mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}\right)\\\left(\mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}\right) \mathbf{B}^T&\mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}\end{smallmatrix}\right]\\
\end{align*}

9. p(xt|y(1:t))



In [16]:

    
p_xt_update = p_yt_xt.condition([y_t])

print("p_xt_update:")
display(Latex(utils.matLatex(p_xt_update)))









    



S.blockform:  [[S_(y_t,y_t)]]
S.variables_dim1:  {y_t: 0}
S.variables_dim2:  {y_t: 0}
old_mean:  [A*m_{t|t} + b_t]
P: [[S_(x_t,x_t)]], Q: [[S_(x_t,y_t)]], R: [[S_(y_t,x_t)]], S: [[S_(y_t,y_t)]]
v_marg_vars:  [y_t]
marg_mean:  [B*(A*m_{t|t} + b_t) + d_t]
p_xt_update:






    





\begin{align*}
p\left(\mathbf{x_t}|\mathbf{y_{1:t-1}},\mathbf{y_t}\right)&= \mathcal{N}\left(\mathbf{m}_{\mathbf{x_t}|\mathbf{y_{1:t-1}},\mathbf{y_t}},\mathbf{\Sigma}_{\mathbf{x_t}|\mathbf{y_{1:t-1}},\mathbf{y_t}}\right)\\
\mathbf{m}_{\mathbf{x_t}|\mathbf{y_{1:t-1}},\mathbf{y_t}} &= \mathbf{A} \mu_{\mathbf{x_{t-1}}} + \left(\mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}\right) \mathbf{B}^T \left(\mathbf{B} \left(\mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}\right) \mathbf{B}^T + \Sigma_{\mathbf{e_t}}\right)^{-1} \left(-1 \left(\mathbf{B} \left(\mathbf{A} \mu_{\mathbf{x_{t-1}}} + \mathbf{b_t}\right) + \mathbf{d_t}\right) + \mathbf{y_t}\right) + \mathbf{b_t}\\
\mathbf{\Sigma}_{\mathbf{x_t}|\mathbf{y_{1:t-1}},\mathbf{y_t}} &= \mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T - \left(\mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}\right) \mathbf{B}^T \left(\mathbf{B} \left(\mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}\right) \mathbf{B}^T + \Sigma_{\mathbf{e_t}}\right)^{-1} \mathbf{B} \left(\mathbf{A} \Sigma_{\mathbf{x_{t-1}}} \mathbf{A}^T + \Sigma_{\mathbf{w_t}}\right) + \Sigma_{\mathbf{w_t}}\\
\end{align*}