# Demo: Generalised Filtering with eXogenous input

Wouter Kouw, 03-08-2021

---


Consider a discrete-time dynamic system with states $x_k \in \mathbb{R}^{d_x}$ and observations $y_k \in \mathbb{R}^{d_y}$. 

We have the following state-space model:

$$\begin{align}
z_k =&\ A(\theta) z_{k-1} + B(\eta) u_k + q_{k-1} \\
y_k =&\ x_k + r_{k} \, ,
\end{align}$$

where $A$ is a state transition matrix, $C$ is an observation emission matrix, $q_{k-1} \sim \mathcal{N}(0, Q)$ is Gaussian distributed process noise and $r_{k} \sim \mathcal{N}(0,R)$ is Gaussian distributed measurement noise.

### System

This project considers a [harmonic oscillator](https://en.wikipedia.org/wiki/Harmonic_oscillator#Driven_harmonic_oscillators). Consider $y(t)$ as observed displacement, $x(t)$ as the unobserved state, $u(t)$ as the driving force and $v(t), w(t)$ as measurement and process noise, respectively. The continuous-time dynamics of the system are:

$$\begin{align}
m \frac{d^2 x(t)}{dt^2} =&\ - c \frac{d x(t)}{dt} - k x(t) + u(t) \\
y(t) =&\ x(t) + e(t)
\end{align}$$
where 
$$
m     = \text{mass} \, , \quad
c     = \text{damping} \, , \quad
k     = \text{spring stiffness} \, ,
$$
constitute the physical parameters.

The process noise is a Wiener process, where the increment is Gaussian distributed $w(t) \sim \mathcal{N}(0, \zeta^{-1}dt)$ with $\zeta$ representing the precision of the process. The measurement noise is also a Wiener process, with $v(t) \sim \mathcal{N}(0, \xi^{-1}dt)$ and $\xi$ as precision parameter.

We cast this to the following discrete-time system. Using a forward finite difference method, i.e.,

$$\begin{align}
\frac{d^2 x(t)}{dt^2} = \frac{x_{t+1} - 2x_t + x_{t-1}}{(\Delta t)^2} \, , \quad \text{and} \quad
\frac{d x(t)}{dt} = \frac{x_{t+1} - x_t}{\Delta t} \, ,
\end{align}$$

we get:

$$\begin{align}
m \frac{x_{t+1} - 2x_t + x_{t-1}}{(\Delta t)^2}  =& - c \frac{x_{t+1} - x_t}{\Delta t} -  k x_t + u_t \\
x_{t+1} =&\ -\frac{-2m + k (\Delta t)^2}{m + c \Delta t} x_t - \frac{m - c \Delta t}{m + c \Delta t} = \frac{(\Delta t)^2}{m + c \Delta t} u_t \, .
\end{align}$$

If we substitute variables we get:

$$\begin{align} 
x_{t+1} =&\ \theta_1 x_t + \theta_2 x_{t-1} + \eta u_t \\ 
y_t =&\ x_t + v_t \, .
\end{align}$$

#### Data-generating process


```julia
using LinearAlgebra
using ProgressMeter
using Plots
pyplot();
```


```julia
# Length of time-series
T = 100

# Sampling step size
Δt = 0.1

# Coefficients
θ_true = [0.1, 0.2]
η_true = .8

# Utility matrices
S = [1 Δt; 0 1];
s = [0; -Δt];

# Emission matrix
C = [1., 0.]

# Process noise covariance
τ = 4.
Q = inv(τ) .*[Δt^3/3 Δt^2/2;
              Δt^2/2     Δt];
    
# Measurement noise variance  
R = 1e-3
     
# Initial states
x0 = [0., 1.]

# Preallocate data array
states = zeros(2,T)
observations = zeros(1,T)

# Initialize previous state
prev_state = x0

for k = 1:T
    
    # State transition
    states[:,k] = (S + s*θ_true')*prev_state + -s*η_true + cholesky(Q).L*randn(2,)
    
    # Observation with added measurement noise
    observations[1,k] = C'*states[:,k] + sqrt(R)*randn(1,)[1]
    
    # Update "previous state"
    prev_state = states[:,k]
    
end    
```


```julia
# Inspect data
p1 = plot((1:T).*Δt, states[1,:], linewidth=3, color="blue", xlabel="", ylabel="x", label="states")
scatter!((1:T).*Δt, observations[1,:], color="black", markersize=2, label="observed", legend=:topleft)
p2 = plot((1:T).*Δt, states[2,:], linewidth=3, color="blue", xlabel="time [Δt="*string(Δt)*" seconds]", ylabel="x'", label="states")
plot(p1,p2, layout=(2,1), size=(900,300))
```

### Model specification

It is often not trivial to formulate a probabilistic model for a given problem. In the case of a linear Gaussian dynamical system, it is fortunately relatively straightforward.

We can convert the state transition and observation likelihood from the system dynamics to probabilistic form by integrating out the noise variables:

$$\begin{align}
x_k \sim&\ \mathcal{N}(Ax_{k-1}, Q) \\
y_k \sim&\ \mathcal{N}(Cx_k, R) \, .
\end{align}$$

The noise covariance matrices are now the covariance matrices of the stochastic state transition and likelihood. In ForneyLab, we often work with precision matrices instead of covariance matrices as that tends to require less matrix inversions.

We will need priors for the unknown variables. We will be doing recursive estimation (filtering) in this notebook, we need only define a prior for the "previous state" variable $x_{k-1}$:

$$\begin{align} 
x_{k-1} \sim \mathcal{N}(m_k, W_k^{-1}) \, ,
\end{align}$$

where $W_k$ is a precision matrix. We will model the unknown noise precision matrices with Wishart distributions since these are the conditionally [conjugate priors](https://en.wikipedia.org/wiki/Conjugate_prior) for Gaussian likelihoods:

$$\begin{align} 
Q^{-1} \sim&\ \mathcal{W}(V_Q, n_Q) \\
R^{-1} \sim&\ \mathcal{W}(V_R, n_R) \, .
\end{align}$$

We now have a likelihood and priors. That is all we need for ForneyLab.


```julia
using ForneyLab
```


```julia
fg = FactorGraph()

# Measurement noise precision (R inverse)
@RV Ri ~ Wishart(placeholder(:V_R, dims=(2,2)), placeholder(:n_R))

# Process noise precision (Q inverse)
@RV Qi ~ Wishart(placeholder(:V_Q, dims=(4,4)), placeholder(:n_Q))

# Previous state vector
@RV x_kmin1 ~ GaussianMeanPrecision(placeholder(:m_k, dims=(4,)), placeholder(:W_k, dims=(4,4)))

# State transition
@RV x_k ~ GaussianMeanPrecision(A*x_kmin1, Qi)

# Observation likelihood
@RV y_k ~ GaussianMeanPrecision(C*x_k, Ri)

# Indicate y_k will be observed
placeholder(y_k, :y_k, dims=(2,));

ForneyLab.draw(fg)
```


<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN"
 "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
<!-- Generated by graphviz version 2.47.1 (20210417.1919)
 -->
<!-- Title: G Pages: 1 -->
<svg width="795pt" height="746pt"
 viewBox="0.00 0.00 794.50 746.00" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink">
<g id="graph0" class="graph" transform="scale(1 1) rotate(0) translate(4 742)">
<title>G</title>
<polygon fill="white" stroke="transparent" points="-4,4 -4,-742 790.5,-742 790.5,4 -4,4"/>
<!-- 7836607220857614326 -->
<g id="node1" class="node">
<title>7836607220857614326</title>
<polygon fill="lightgrey" stroke="black" points="488.5,-738 411.5,-738 411.5,-684 488.5,-684 488.5,-738"/>
<text text-anchor="middle" x="450" y="-708.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_y_k</text>
</g>
<!-- 1646826350774227711 -->
<g id="node11" class="node">
<title>1646826350774227711</title>
<polygon fill="none" stroke="black" points="503.5,-639 396.5,-639 396.5,-567 503.5,-567 503.5,-639"/>
<text text-anchor="middle" x="450" y="-605.8" font-family="Times New Roman,serif" font-size="9.00">𝒩</text>
<text text-anchor="middle" x="450" y="-595.8" font-family="Times New Roman,serif" font-size="9.00">gaussianmeanprecision_3</text>
</g>
<!-- 7836607220857614326&#45;&#45;1646826350774227711 -->
<g id="edge10" class="edge">
<title>7836607220857614326&#45;&#45;1646826350774227711</title>
<path fill="none" stroke="black" d="M450,-683.8C450,-670.31 450,-653.68 450,-639.1"/>
<text text-anchor="start" x="450" y="-659.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">y_k</text>
<text text-anchor="start" x="429" y="-641.7" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="429" y="-677.4" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
</g>
<!-- 5443584827191874034 -->
<g id="node2" class="node">
<title>5443584827191874034</title>
<polygon fill="lightgrey" stroke="black" points="385,-54 305,-54 305,0 385,0 385,-54"/>
<text text-anchor="middle" x="345" y="-24.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_W_k</text>
</g>
<!-- 14074949235902030186 -->
<g id="node3" class="node">
<title>14074949235902030186</title>
<polygon fill="lightgrey" stroke="black" points="592.5,-396 515.5,-396 515.5,-342 592.5,-342 592.5,-396"/>
<text text-anchor="middle" x="554" y="-366.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_n_R</text>
</g>
<!-- 1827920715093087870 -->
<g id="node4" class="node">
<title>1827920715093087870</title>
<polygon fill="lightgrey" stroke="black" points="178,-396 124,-396 124,-342 178,-342 178,-396"/>
<text text-anchor="middle" x="151" y="-366.8" font-family="Times New Roman,serif" font-size="9.00">clamp_2</text>
</g>
<!-- 14907013347372185915 -->
<g id="node5" class="node">
<title>14907013347372185915</title>
<polygon fill="lightgrey" stroke="black" points="578,-54 500,-54 500,0 578,0 578,-54"/>
<text text-anchor="middle" x="539" y="-24.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_m_k</text>
</g>
<!-- 13282723035730671914 -->
<g id="node6" class="node">
<title>13282723035730671914</title>
<polygon fill="lightgrey" stroke="black" points="80,-162 0,-162 0,-108 80,-108 80,-162"/>
<text text-anchor="middle" x="40" y="-132.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_V_Q</text>
</g>
<!-- 4433327480983599190 -->
<g id="node7" class="node">
<title>4433327480983599190</title>
<polygon fill="lightgrey" stroke="black" points="273,-162 195,-162 195,-108 273,-108 273,-162"/>
<text text-anchor="middle" x="234" y="-132.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_n_Q</text>
</g>
<!-- 5888902112194352978 -->
<g id="node8" class="node">
<title>5888902112194352978</title>
<polygon fill="none" stroke="black" points="385,-522 309,-522 309,-450 385,-450 385,-522"/>
<text text-anchor="middle" x="347" y="-488.8" font-family="Times New Roman,serif" font-size="9.00">×</text>
<text text-anchor="middle" x="347" y="-478.8" font-family="Times New Roman,serif" font-size="9.00">multiplication_2</text>
</g>
<!-- 5888902112194352978&#45;&#45;1827920715093087870 -->
<g id="edge14" class="edge">
<title>5888902112194352978&#45;&#45;1827920715093087870</title>
<path fill="none" stroke="black" d="M308.74,-462.55C270.31,-440 212.05,-405.82 178.13,-385.92"/>
<text text-anchor="start" x="255" y="-425.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">clamp_2</text>
<text text-anchor="start" x="178.13" y="-388.52" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="295.74" y="-465.15" font-family="Times New Roman,serif" font-size="8.00">3 a </text>
</g>
<!-- 15831686215905286561 -->
<g id="node16" class="node">
<title>15831686215905286561</title>
<polygon fill="none" stroke="black" points="400.5,-405 293.5,-405 293.5,-333 400.5,-333 400.5,-405"/>
<text text-anchor="middle" x="347" y="-371.8" font-family="Times New Roman,serif" font-size="9.00">𝒩</text>
<text text-anchor="middle" x="347" y="-361.8" font-family="Times New Roman,serif" font-size="9.00">gaussianmeanprecision_2</text>
</g>
<!-- 5888902112194352978&#45;&#45;15831686215905286561 -->
<g id="edge6" class="edge">
<title>5888902112194352978&#45;&#45;15831686215905286561</title>
<path fill="none" stroke="black" d="M347,-449.87C347,-435.74 347,-419.46 347,-405.31"/>
<text text-anchor="start" x="347" y="-425.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">x_k</text>
<text text-anchor="start" x="326" y="-407.91" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="326" y="-443.47" font-family="Times New Roman,serif" font-size="8.00">2 in1 </text>
</g>
<!-- 3202067625536609345 -->
<g id="node9" class="node">
<title>3202067625536609345</title>
<polygon fill="none" stroke="black" points="590,-522 518,-522 518,-450 590,-450 590,-522"/>
<text text-anchor="middle" x="554" y="-488.8" font-family="Times New Roman,serif" font-size="9.00">W</text>
<text text-anchor="middle" x="554" y="-478.8" font-family="Times New Roman,serif" font-size="9.00">wishart_1</text>
</g>
<!-- 3202067625536609345&#45;&#45;14074949235902030186 -->
<g id="edge7" class="edge">
<title>3202067625536609345&#45;&#45;14074949235902030186</title>
<path fill="none" stroke="black" d="M554,-449.87C554,-432.57 554,-412.06 554,-396.15"/>
<text text-anchor="start" x="554" y="-425.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">n_R</text>
<text text-anchor="start" x="533" y="-398.75" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="536" y="-443.47" font-family="Times New Roman,serif" font-size="8.00">3 nu </text>
</g>
<!-- 6714325340166659249 -->
<g id="node12" class="node">
<title>6714325340166659249</title>
<polygon fill="lightgrey" stroke="black" points="786.5,-396 707.5,-396 707.5,-342 786.5,-342 786.5,-396"/>
<text text-anchor="middle" x="747" y="-366.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_V_R</text>
</g>
<!-- 3202067625536609345&#45;&#45;6714325340166659249 -->
<g id="edge13" class="edge">
<title>3202067625536609345&#45;&#45;6714325340166659249</title>
<path fill="none" stroke="black" d="M590.3,-463.37C623.61,-443.52 672.84,-414.19 707.2,-393.71"/>
<text text-anchor="start" x="656" y="-425.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">V_R</text>
<text text-anchor="start" x="686.2" y="-396.31" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="590.3" y="-465.97" font-family="Times New Roman,serif" font-size="8.00">2 v </text>
</g>
<!-- 2042325482365814062 -->
<g id="node10" class="node">
<title>2042325482365814062</title>
<polygon fill="none" stroke="black" points="495.5,-171 388.5,-171 388.5,-99 495.5,-99 495.5,-171"/>
<text text-anchor="middle" x="442" y="-137.8" font-family="Times New Roman,serif" font-size="9.00">𝒩</text>
<text text-anchor="middle" x="442" y="-127.8" font-family="Times New Roman,serif" font-size="9.00">gaussianmeanprecision_1</text>
</g>
<!-- 2042325482365814062&#45;&#45;5443584827191874034 -->
<g id="edge12" class="edge">
<title>2042325482365814062&#45;&#45;5443584827191874034</title>
<path fill="none" stroke="black" d="M409.97,-99C396.62,-84.4 381.37,-67.75 369.01,-54.23"/>
<text text-anchor="start" x="394" y="-74.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">W_k</text>
<text text-anchor="start" x="348.01" y="-56.83" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="394.97" y="-92.6" font-family="Times New Roman,serif" font-size="8.00">3 w </text>
</g>
<!-- 2042325482365814062&#45;&#45;14907013347372185915 -->
<g id="edge8" class="edge">
<title>2042325482365814062&#45;&#45;14907013347372185915</title>
<path fill="none" stroke="black" d="M474.03,-99C487.38,-84.4 502.63,-67.75 514.99,-54.23"/>
<text text-anchor="start" x="497" y="-74.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">m_k</text>
<text text-anchor="start" x="493.99" y="-56.83" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="458.03" y="-92.6" font-family="Times New Roman,serif" font-size="8.00">2 m </text>
</g>
<!-- 1646826350774227711&#45;&#45;5888902112194352978 -->
<g id="edge9" class="edge">
<title>1646826350774227711&#45;&#45;5888902112194352978</title>
<path fill="none" stroke="black" d="M396.4,-579.48C383.11,-571.6 370.09,-561.52 361,-549 355.36,-541.23 351.94,-531.58 349.87,-522.23"/>
<text text-anchor="start" x="361" y="-542.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">variable_2</text>
<text text-anchor="start" x="328.87" y="-524.83" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="380.4" y="-582.08" font-family="Times New Roman,serif" font-size="8.00">2 m </text>
</g>
<!-- 1646826350774227711&#45;&#45;3202067625536609345 -->
<g id="edge5" class="edge">
<title>1646826350774227711&#45;&#45;3202067625536609345</title>
<path fill="none" stroke="black" d="M481.77,-566.87C494.55,-552.74 509.27,-536.46 522.07,-522.31"/>
<text text-anchor="start" x="506" y="-542.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">Ri</text>
<text text-anchor="start" x="501.07" y="-524.91" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="466.77" y="-560.47" font-family="Times New Roman,serif" font-size="8.00">3 w </text>
</g>
<!-- 2265991541729979615 -->
<g id="node13" class="node">
<title>2265991541729979615</title>
<polygon fill="none" stroke="black" points="270,-288 198,-288 198,-216 270,-216 270,-288"/>
<text text-anchor="middle" x="234" y="-254.8" font-family="Times New Roman,serif" font-size="9.00">W</text>
<text text-anchor="middle" x="234" y="-244.8" font-family="Times New Roman,serif" font-size="9.00">wishart_2</text>
</g>
<!-- 2265991541729979615&#45;&#45;13282723035730671914 -->
<g id="edge11" class="edge">
<title>2265991541729979615&#45;&#45;13282723035730671914</title>
<path fill="none" stroke="black" d="M197.96,-229.64C164.54,-209.83 114.89,-180.39 80.19,-159.83"/>
<text text-anchor="start" x="143" y="-191.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">V_Q</text>
<text text-anchor="start" x="80.19" y="-162.43" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="183.96" y="-232.24" font-family="Times New Roman,serif" font-size="8.00">2 v </text>
</g>
<!-- 2265991541729979615&#45;&#45;4433327480983599190 -->
<g id="edge3" class="edge">
<title>2265991541729979615&#45;&#45;4433327480983599190</title>
<path fill="none" stroke="black" d="M234,-215.87C234,-198.57 234,-178.06 234,-162.15"/>
<text text-anchor="start" x="234" y="-191.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">n_Q</text>
<text text-anchor="start" x="213" y="-164.75" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="216" y="-209.47" font-family="Times New Roman,serif" font-size="8.00">3 nu </text>
</g>
<!-- 6062987519380755961 -->
<g id="node14" class="node">
<title>6062987519380755961</title>
<polygon fill="lightgrey" stroke="black" points="665,-162 611,-162 611,-108 665,-108 665,-162"/>
<text text-anchor="middle" x="638" y="-132.8" font-family="Times New Roman,serif" font-size="9.00">clamp_1</text>
</g>
<!-- 520186944529735059 -->
<g id="node15" class="node">
<title>520186944529735059</title>
<polygon fill="none" stroke="black" points="480,-288 404,-288 404,-216 480,-216 480,-288"/>
<text text-anchor="middle" x="442" y="-254.8" font-family="Times New Roman,serif" font-size="9.00">×</text>
<text text-anchor="middle" x="442" y="-244.8" font-family="Times New Roman,serif" font-size="9.00">multiplication_1</text>
</g>
<!-- 520186944529735059&#45;&#45;2042325482365814062 -->
<g id="edge1" class="edge">
<title>520186944529735059&#45;&#45;2042325482365814062</title>
<path fill="none" stroke="black" d="M442,-215.87C442,-201.74 442,-185.46 442,-171.31"/>
<text text-anchor="start" x="442" y="-191.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">x_kmin1</text>
<text text-anchor="start" x="421" y="-173.91" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="421" y="-209.47" font-family="Times New Roman,serif" font-size="8.00">2 in1 </text>
</g>
<!-- 520186944529735059&#45;&#45;6062987519380755961 -->
<g id="edge2" class="edge">
<title>520186944529735059&#45;&#45;6062987519380755961</title>
<path fill="none" stroke="black" d="M480.26,-228.55C518.69,-206 576.95,-171.82 610.87,-151.92"/>
<text text-anchor="start" x="546" y="-191.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">clamp_1</text>
<text text-anchor="start" x="589.87" y="-154.52" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="480.26" y="-231.15" font-family="Times New Roman,serif" font-size="8.00">3 a </text>
</g>
<!-- 15831686215905286561&#45;&#45;2265991541729979615 -->
<g id="edge15" class="edge">
<title>15831686215905286561&#45;&#45;2265991541729979615</title>
<path fill="none" stroke="black" d="M312.48,-332.87C298.59,-318.74 282.6,-302.46 268.7,-288.31"/>
<text text-anchor="start" x="294" y="-308.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">Qi</text>
<text text-anchor="start" x="247.7" y="-290.91" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="297.48" y="-326.47" font-family="Times New Roman,serif" font-size="8.00">3 w </text>
</g>
<!-- 15831686215905286561&#45;&#45;520186944529735059 -->
<g id="edge4" class="edge">
<title>15831686215905286561&#45;&#45;520186944529735059</title>
<path fill="none" stroke="black" d="M376.02,-332.87C387.7,-318.74 401.14,-302.46 412.83,-288.31"/>
<text text-anchor="start" x="398" y="-308.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">variable_1</text>
<text text-anchor="start" x="391.83" y="-290.91" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="360.02" y="-326.47" font-family="Times New Roman,serif" font-size="8.00">2 m </text>
</g>
</g>
</svg>



### Compile inference algorithm

We cannot use exact Bayesian inference here and will therefore switch to an approximate inference scheme: variational message passing.


```julia
# Define a factorization for the posterior
q = PosteriorFactorization(x_k, x_kmin1, Qi, Ri, ids=[:x_k, :x_kmin1, :Q, :R])

# Define a variational message passing procedure
algorithm = messagePassingAlgorithm([x_k, x_kmin1, Qi, Ri], q)

# Compile message passing procedure to an inference algorithm
code = algorithmSourceCode(algorithm);

# Import compiled functions to workspace
eval(Meta.parse(code));

# Print compiled functions (uncomment when desired)
# println(code)
```

### Execute inference algorithm

We execute the algorithm in an online fashion: after each timestep the posteriors for the state and noise precisions are used as priors for the next timestep.


```julia
# Parameters for prior distributions
m_x_0 = [0., 0., 1., -1.]
W_x_0 = 10. .* Matrix{Float64}(I, 4,4)
V_Q_0 = 10 .*Matrix{Float64}(I, 4,4)
n_Q_0 = 1.0
V_R_0 = Matrix{Float64}(I, 2,2)
n_R_0 = 100.0;
```


```julia
# Number of iterations
num_iterations = 5

# Initialize arrays for storing parameter estimates
m_x_k = zeros(4,T)
W_x_k = zeros(4,4,T)
V_Q_k = zeros(4,4,T)
n_Q_k = zeros(T,)
V_R_k = zeros(2,2,T)
n_R_k = zeros(T,)

# Initialize previous parameter estimates
m_x_kmin1 = m_x_0
W_x_kmin1 = W_x_0
V_Q_kmin1 = V_Q_0
n_Q_kmin1 = n_Q_0
V_R_kmin1 = V_R_0
n_R_kmin1 = n_R_0

# Initialize data dictionary
data = Dict()

# Initialize marginal distributions
marginals = Dict()
marginals[:x_kmin1] = ProbabilityDistribution(Multivariate, GaussianMeanPrecision, m=m_x_0, w=W_x_0)
marginals[:x_k] = ProbabilityDistribution(Multivariate, GaussianMeanPrecision, m=m_x_0, w=W_x_0)
marginals[:Qi] = ProbabilityDistribution(Wishart, v=V_Q_0, nu=n_Q_0)
marginals[:Ri] = ProbabilityDistribution(Wishart, v=V_R_0, nu=n_R_0)

# Recursive estimation procedure (posteriors at k => priors at k+1)
@showprogress for k = 1:T
    
    # Store data for current time-step
    data[:y_k] = observations[:,k]
    data[:m_k] = m_x_kmin1
    data[:W_k] = W_x_kmin1
    data[:V_Q] = V_Q_kmin1
    data[:n_Q] = n_Q_kmin1
    data[:V_R] = V_R_kmin1
    data[:n_R] = n_R_kmin1
    
    # Coordinate descent
    for kk = 1:num_iterations
        
        # Update recognition factors
        stepx_kmin1!(data, marginals)
        stepx_k!(data, marginals)
        stepQ!(data, marginals)
        stepR!(data, marginals)
        
    end
    
    # Update book-keeping
    m_x_kmin1 = m_x_k[:,k] = ForneyLab.unsafeMean(marginals[:x_k])
    W_x_kmin1 = W_x_k[:,:,k] = ForneyLab.unsafePrecision(marginals[:x_k])
    V_Q_kmin1 = V_Q_k[:,:,k] = marginals[:Qi].params[:v]
    n_Q_kmin1 = n_Q_k[k] = marginals[:Qi].params[:nu]
    V_R_kmin1 = V_R_k[:,:,k] = marginals[:Ri].params[:v]
    n_R_kmin1 = n_R_k[k] = marginals[:Ri].params[:nu]
    
end
```

    [32mProgress: 100%|█████████████████████████████████████████| Time: 0:00:06[39m


### Inspect results
We can plot the estimates of the position and velocity over time, including the uncertainty of the estimates.


```julia
# Inspect data
p1 = plot((1:T).*Δt, states[1,:], linewidth=2, color="blue", xlabel="", ylabel="position 1", label="states")
scatter!((1:T).*Δt, observations[1,:], color="black", markersize=2, label="observed", legend=:topleft, title="2D position of car over time")
plot!((1:T).*Δt, m_x_k[1,:], linewidth=3, color="red", ribbon=[sqrt.(inv.(W_x_k[1,1,:])) sqrt.(inv.(W_x_k[1,1,:]))], alpha=0.6, label="estimated")
p2 = plot((1:T).*Δt, states[2,:], linewidth=2, color="blue", xlabel="time [Δt="*string(Δt)*" seconds]", ylabel="position 2", label="states")
scatter!((1:T).*Δt, observations[2,:], color="black", markersize=2, label="observed", legend=:bottomleft)
plot!((1:T).*Δt, m_x_k[2,:], linewidth=3, color="red", ribbon=[sqrt.(inv.(W_x_k[2,2,:])) sqrt.(inv.(W_x_k[2,2,:]))], alpha=0.6, label="estimated")
plot(p1,p2, layout=(2,1), size=(700,400))
```


```julia
# Inspect data
p1 = plot((1:T).*Δt, states[3,:], linewidth=2, color="blue", xlabel="", ylabel="velocity 1", label="states", title="2D velocity of car over time")
plot!((1:T).*Δt, m_x_k[3,:], linewidth=3, color="red", ribbon=[sqrt.(inv.(W_x_k[3,3,:])) sqrt.(inv.(W_x_k[3,3,:]))], alpha=0.6, label="estimated")
p2 = plot((1:T).*Δt, states[4,:], linewidth=2, color="blue", xlabel="time [Δt="*string(Δt)*" seconds]", ylabel="velocity 2", label="states")
plot!((1:T).*Δt, m_x_k[4,:], linewidth=3, color="red", ribbon=[sqrt.(inv.(W_x_k[4,4,:])) sqrt.(inv.(W_x_k[4,4,:]))], alpha=0.6, label="estimated")
plot(p1,p2, layout=(2,1), size=(700,400))
```


```julia

```

c495da7e8c1b119f7aa3c9219026a1f4b3b7e4cc
ipynb
Jupyter Notebook
GFX node.ipynb
wmkouw/gfx-node
0a1e3c62b2dc619582d95162c990f640e52e8052
MIT
2021-05-31T16:06:14.000Z
2021-05-31T16:06:14.000Z
GFX node.ipynb
wmkouw/GFX
0a1e3c62b2dc619582d95162c990f640e52e8052
MIT
GFX node.ipynb
wmkouw/GFX
0a1e3c62b2dc619582d95162c990f640e52e8052
MIT
Qwen/Qwen-72B
1. YES
2. YES