# Bayesian filtering for nonlinear Gaussian dynamical systems

Wouter Kouw | Last update: 09-12-2021

Consider a discrete-time dynamic system with observations $y_k \in \mathbb{R}^{d_y}$ and latent states $x_k \in \mathbb{R}^{d_x}$. These are subject to the following dynamics:

$$\begin{align}
x_k =&\ f(x_{k-1}) + q_{k-1} \\
y_k =&\ h(x_k) + r_{k} \, ,
\end{align}$$

where $f$ and $h$ are static nonlinear functions, $q_{k-1}$ represents process noise and $r_{k}$ is measurement noise. Both sources of noise are Gaussian distributed with zero mean:

$$q_{k-1} \sim \mathcal{N}(0, Q) \, , \qquad r_k \sim \mathcal{N}(0, R) \, ,$$

with covariance matrices $Q$ and $R$.

## Problem: tracking a noisy pendulum

We will be tracking a pendulum whose observations are noisy. You can find this example in [Bayesian Filtering & Smoothing](https://www.cambridge.org/core/books/bayesian-filtering-and-smoothing/C372FB31C5D9A100F8476C1B23721A67) by Simon Särkkä (Ex 3.7). The state transition has the following form:

$$\begin{align*}
\underbrace{\begin{bmatrix} x_{1,k} \\ x_{2,k} \end{bmatrix}}_{x_k} = \underbrace{\begin{bmatrix} x_{1,k-1} + x_{2,k-1} \Delta t \\ x_{2,k-1} -G \sin(x_{1,k-1}) \Delta t \end{bmatrix}}_{f(x_{k-1})} + q_{k-1}
\end{align*}$$

where $x_1$ represents the angle of the pendulum, $x_2$ represents the change in angle and $G$ is gravitational acceleration. 

The measurement function is described as:

$$\begin{align*}
y_k = \underbrace{\sin(x_{1,k})}_{h(x_k)} + r_k
\end{align*}$$

We cannot observe the change in angle directly, only the height of the angle itself (i.e. $\sin(x_{1})$).

#### Data-generating process


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


```julia
# Length of time-series
T = 400
Δt = 0.01

# Gravitational acceleration
G = 9.81

# Process noise covariance
qc = 1e-3
Q = [qc*Δt^3/3 qc*Δt^2/2; qc*Δt^2/2 qc*Δt]
    
# Measurement noise variance  
R = 1e-2
     
# Initial states
x0 = [1.0, 0.0]

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

# Initialize previous state variable
prev_state = x0

for k = 1:T
    
    # State transition
    states[1,k] = prev_state[1] + prev_state[2]*Δt
    states[2,k] = prev_state[2] - G*sin(prev_state[1])*Δt
    
    # Add process noise
    states[:,k] = states[:,k] + cholesky(Q).L*randn(2)
    
    # Observation with added measurement noise
    observations[k] = sin(states[1,k]) + sqrt(R)*randn(1)[1]
    
    # Update "previous state"
    prev_state = states[:,k]
    
end    
```


```julia
# Inspect data
plot((1:T).*Δt, states[1,:], linewidth=3, color="blue", xlabel="time [Δt = "*string(Δt)*"]", ylabel="height pendulum", label="states", size=(800,400))
scatter!((1:T).*Δt, observations, color="black", markersize=2,label="observed")
```

### Model specification

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}(f(x_{k-1}), Q) \\
y_k \sim&\ \mathcal{N}(g(x_k), R) \, .
\end{align}$$

The noise covariance matrices are now the covariance matrices of the stochastic state transition and likelihood. In both cases, we will use an unscented transform to approximate the result of applying the nonlinear function with a Gaussian distribution.

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. In Bayesian inference, we often work with precision matrices instead of covariance matrices as that tends to require less matrix inversions. We will model the unknown noise precision matrices with a gamma distribution (univariate measurement noise) and a Wishart distribution (multivariate process noise). 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&\ \Gamma(a_r, b_r) \, .
\end{align}$$

With a likelihood and a set of priors, we can specify a model in a probabilistic programming language (here [ForneyLab.jl](https://github.com/biaslab/ForneyLab.jl)) and automatically infer (approximate) posteriors.


```julia
using ForneyLab
```


```julia
fg = FactorGraph()

# Measurement noise precision
@RV Ri ~ Gamma(placeholder(:a_R), placeholder(:b_R))

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

# Previous state vector
@RV x_kmin1 ~ GaussianMeanPrecision(placeholder(:m_x, dims=(2,)), placeholder(:W_x, dims=(2,2)))

# Nonlinear state transition function
f(x) = [x[1] + x[2]*Δt, x[2] - G*sin(x[1])*Δt]
@RV fx_k ~ Nonlinear{Unscented}(x_kmin1; g=f)

# Add process noise
@RV x_k ~ GaussianMeanPrecision(fx_k, Qi)

# Mask the state vector
@RV x_1 = dot([1., 0.], x_k)

# Nonlinear observation function
h(x) = sin(x)
@RV hx_k ~ Nonlinear{Unscented}(x_1; g=h)

# Add measurement noise
@RV y_k ~ GaussianMeanPrecision(hx_k, Ri)

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

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="585pt" height="863pt"
 viewBox="0.00 0.00 584.50 863.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 859)">
<title>G</title>
<polygon fill="white" stroke="transparent" points="-4,4 -4,-859 580.5,-859 580.5,4 -4,4"/>
<!-- 11769799603831642054 -->
<g id="node1" class="node">
<title>11769799603831642054</title>
<polygon fill="none" stroke="black" points="187.5,-288 115.5,-288 115.5,-216 187.5,-216 187.5,-288"/>
<text text-anchor="middle" x="151.5" y="-254.8" font-family="Times New Roman,serif" font-size="9.00">g{Unscented}</text>
<text text-anchor="middle" x="151.5" y="-244.8" font-family="Times New Roman,serif" font-size="9.00">nonlinear_1</text>
</g>
<!-- 7200964442915660904 -->
<g id="node13" class="node">
<title>7200964442915660904</title>
<polygon fill="none" stroke="black" points="188,-171 81,-171 81,-99 188,-99 188,-171"/>
<text text-anchor="middle" x="134.5" y="-137.8" font-family="Times New Roman,serif" font-size="9.00">𝒩</text>
<text text-anchor="middle" x="134.5" y="-127.8" font-family="Times New Roman,serif" font-size="9.00">gaussianmeanprecision_1</text>
</g>
<!-- 11769799603831642054&#45;&#45;7200964442915660904 -->
<g id="edge9" class="edge">
<title>11769799603831642054&#45;&#45;7200964442915660904</title>
<path fill="none" stroke="black" d="M146.31,-215.87C144.22,-201.74 141.81,-185.46 139.72,-171.31"/>
<text text-anchor="start" x="144.5" y="-191.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">x_kmin1</text>
<text text-anchor="start" x="118.72" y="-173.91" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="125.31" y="-209.47" font-family="Times New Roman,serif" font-size="8.00">2 in1 </text>
</g>
<!-- 15707164406108971955 -->
<g id="node2" class="node">
<title>15707164406108971955</title>
<polygon fill="none" stroke="black" points="167.5,-522 95.5,-522 95.5,-450 167.5,-450 167.5,-522"/>
<text text-anchor="middle" x="131.5" y="-488.8" font-family="Times New Roman,serif" font-size="9.00">dot</text>
<text text-anchor="middle" x="131.5" y="-478.8" font-family="Times New Roman,serif" font-size="9.00">dotproduct_1</text>
</g>
<!-- 13099004617794688251 -->
<g id="node8" class="node">
<title>13099004617794688251</title>
<polygon fill="lightgrey" stroke="black" points="83.5,-396 29.5,-396 29.5,-342 83.5,-342 83.5,-396"/>
<text text-anchor="middle" x="56.5" y="-366.8" font-family="Times New Roman,serif" font-size="9.00">clamp_1</text>
</g>
<!-- 15707164406108971955&#45;&#45;13099004617794688251 -->
<g id="edge5" class="edge">
<title>15707164406108971955&#45;&#45;13099004617794688251</title>
<path fill="none" stroke="black" d="M95.12,-469.18C79.98,-460.55 63.98,-448.26 55.5,-432 49.82,-421.12 49.4,-407.63 50.68,-396.02"/>
<text text-anchor="start" x="55.5" y="-425.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">clamp_1</text>
<text text-anchor="start" x="29.68" y="-398.62" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="74.12" y="-471.78" font-family="Times New Roman,serif" font-size="8.00">3 in2 </text>
</g>
<!-- 12615399910196930301 -->
<g id="node14" class="node">
<title>12615399910196930301</title>
<polygon fill="none" stroke="black" points="306,-405 199,-405 199,-333 306,-333 306,-405"/>
<text text-anchor="middle" x="252.5" y="-371.8" font-family="Times New Roman,serif" font-size="9.00">𝒩</text>
<text text-anchor="middle" x="252.5" y="-361.8" font-family="Times New Roman,serif" font-size="9.00">gaussianmeanprecision_2</text>
</g>
<!-- 15707164406108971955&#45;&#45;12615399910196930301 -->
<g id="edge10" class="edge">
<title>15707164406108971955&#45;&#45;12615399910196930301</title>
<path fill="none" stroke="black" d="M167.8,-450.5C182.84,-436.21 200.26,-419.65 215.38,-405.28"/>
<text text-anchor="start" x="196.5" y="-425.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">x_k</text>
<text text-anchor="start" x="194.38" y="-407.88" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="146.8" y="-444.1" font-family="Times New Roman,serif" font-size="8.00">2 in1 </text>
</g>
<!-- 7572076714573308162 -->
<g id="node3" class="node">
<title>7572076714573308162</title>
<polygon fill="lightgrey" stroke="black" points="383.5,-162 303.5,-162 303.5,-108 383.5,-108 383.5,-162"/>
<text text-anchor="middle" x="343.5" y="-132.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_V_Q</text>
</g>
<!-- 18314240599931604027 -->
<g id="node4" class="node">
<title>18314240599931604027</title>
<polygon fill="lightgrey" stroke="black" points="77,-54 0,-54 0,0 77,0 77,-54"/>
<text text-anchor="middle" x="38.5" y="-24.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_m_x</text>
</g>
<!-- 16861794541815506586 -->
<g id="node5" class="node">
<title>16861794541815506586</title>
<polygon fill="none" stroke="black" points="379.5,-288 307.5,-288 307.5,-216 379.5,-216 379.5,-288"/>
<text text-anchor="middle" x="343.5" y="-254.8" font-family="Times New Roman,serif" font-size="9.00">W</text>
<text text-anchor="middle" x="343.5" y="-244.8" font-family="Times New Roman,serif" font-size="9.00">wishart_1</text>
</g>
<!-- 16861794541815506586&#45;&#45;7572076714573308162 -->
<g id="edge8" class="edge">
<title>16861794541815506586&#45;&#45;7572076714573308162</title>
<path fill="none" stroke="black" d="M343.5,-215.87C343.5,-198.57 343.5,-178.06 343.5,-162.15"/>
<text text-anchor="start" x="343.5" y="-191.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">V_Q</text>
<text text-anchor="start" x="322.5" y="-164.75" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="329.5" y="-209.47" font-family="Times New Roman,serif" font-size="8.00">2 v </text>
</g>
<!-- 13333063496959055474 -->
<g id="node15" class="node">
<title>13333063496959055474</title>
<polygon fill="lightgrey" stroke="black" points="576.5,-162 498.5,-162 498.5,-108 576.5,-108 576.5,-162"/>
<text text-anchor="middle" x="537.5" y="-132.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_n_Q</text>
</g>
<!-- 16861794541815506586&#45;&#45;13333063496959055474 -->
<g id="edge1" class="edge">
<title>16861794541815506586&#45;&#45;13333063496959055474</title>
<path fill="none" stroke="black" d="M379.54,-229.64C413.28,-209.63 463.59,-179.81 498.33,-159.22"/>
<text text-anchor="start" x="446.5" y="-191.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">n_Q</text>
<text text-anchor="start" x="477.33" y="-161.82" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="379.54" y="-232.24" font-family="Times New Roman,serif" font-size="8.00">3 nu </text>
</g>
<!-- 14420421744326312879 -->
<g id="node6" class="node">
<title>14420421744326312879</title>
<polygon fill="lightgrey" stroke="black" points="271,-54 192,-54 192,0 271,0 271,-54"/>
<text text-anchor="middle" x="231.5" y="-24.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_W_x</text>
</g>
<!-- 2576489981286358758 -->
<g id="node7" class="node">
<title>2576489981286358758</title>
<polygon fill="lightgrey" stroke="black" points="360,-513 283,-513 283,-459 360,-459 360,-513"/>
<text text-anchor="middle" x="321.5" y="-483.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_b_R</text>
</g>
<!-- 18030336638603525491 -->
<g id="node9" class="node">
<title>18030336638603525491</title>
<polygon fill="none" stroke="black" points="169.5,-639 97.5,-639 97.5,-567 169.5,-567 169.5,-639"/>
<text text-anchor="middle" x="133.5" y="-605.8" font-family="Times New Roman,serif" font-size="9.00">g{Unscented}</text>
<text text-anchor="middle" x="133.5" y="-595.8" font-family="Times New Roman,serif" font-size="9.00">nonlinear_2</text>
</g>
<!-- 18030336638603525491&#45;&#45;15707164406108971955 -->
<g id="edge2" class="edge">
<title>18030336638603525491&#45;&#45;15707164406108971955</title>
<path fill="none" stroke="black" d="M132.89,-566.87C132.64,-552.74 132.36,-536.46 132.11,-522.31"/>
<text text-anchor="start" x="133.5" y="-542.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">x_1</text>
<text text-anchor="start" x="111.11" y="-524.91" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="111.89" y="-560.47" font-family="Times New Roman,serif" font-size="8.00">2 in1 </text>
</g>
<!-- 4789214632737044419 -->
<g id="node10" class="node">
<title>4789214632737044419</title>
<polygon fill="none" stroke="black" points="357.5,-639 285.5,-639 285.5,-567 357.5,-567 357.5,-639"/>
<text text-anchor="middle" x="321.5" y="-605.8" font-family="Times New Roman,serif" font-size="9.00">Gam</text>
<text text-anchor="middle" x="321.5" y="-595.8" font-family="Times New Roman,serif" font-size="9.00">gamma_1</text>
</g>
<!-- 4789214632737044419&#45;&#45;2576489981286358758 -->
<g id="edge11" class="edge">
<title>4789214632737044419&#45;&#45;2576489981286358758</title>
<path fill="none" stroke="black" d="M321.5,-566.87C321.5,-549.57 321.5,-529.06 321.5,-513.15"/>
<text text-anchor="start" x="321.5" y="-542.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">b_R</text>
<text text-anchor="start" x="300.5" y="-515.75" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="308.5" y="-560.47" font-family="Times New Roman,serif" font-size="8.00">3 b </text>
</g>
<!-- 11609603851023295769 -->
<g id="node12" class="node">
<title>11609603851023295769</title>
<polygon fill="lightgrey" stroke="black" points="552,-513 475,-513 475,-459 552,-459 552,-513"/>
<text text-anchor="middle" x="513.5" y="-483.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_a_R</text>
</g>
<!-- 4789214632737044419&#45;&#45;11609603851023295769 -->
<g id="edge14" class="edge">
<title>4789214632737044419&#45;&#45;11609603851023295769</title>
<path fill="none" stroke="black" d="M357.61,-580.37C391.07,-560.33 440.69,-530.61 474.91,-510.11"/>
<text text-anchor="start" x="423.5" y="-542.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">a_R</text>
<text text-anchor="start" x="453.91" y="-512.71" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="357.61" y="-582.97" font-family="Times New Roman,serif" font-size="8.00">2 a </text>
</g>
<!-- 10856907810642589554 -->
<g id="node11" class="node">
<title>10856907810642589554</title>
<polygon fill="lightgrey" stroke="black" points="265,-855 188,-855 188,-801 265,-801 265,-855"/>
<text text-anchor="middle" x="226.5" y="-825.8" font-family="Times New Roman,serif" font-size="9.00">placeholder_y_k</text>
</g>
<!-- 2184084146676702280 -->
<g id="node16" class="node">
<title>2184084146676702280</title>
<polygon fill="none" stroke="black" points="280,-756 173,-756 173,-684 280,-684 280,-756"/>
<text text-anchor="middle" x="226.5" y="-722.8" font-family="Times New Roman,serif" font-size="9.00">𝒩</text>
<text text-anchor="middle" x="226.5" y="-712.8" font-family="Times New Roman,serif" font-size="9.00">gaussianmeanprecision_3</text>
</g>
<!-- 10856907810642589554&#45;&#45;2184084146676702280 -->
<g id="edge7" class="edge">
<title>10856907810642589554&#45;&#45;2184084146676702280</title>
<path fill="none" stroke="black" d="M226.5,-800.8C226.5,-787.31 226.5,-770.68 226.5,-756.1"/>
<text text-anchor="start" x="226.5" y="-776.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">y_k</text>
<text text-anchor="start" x="205.5" y="-758.7" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="205.5" y="-794.4" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
</g>
<!-- 7200964442915660904&#45;&#45;18314240599931604027 -->
<g id="edge4" class="edge">
<title>7200964442915660904&#45;&#45;18314240599931604027</title>
<path fill="none" stroke="black" d="M89.97,-98.74C83.82,-93.09 77.8,-87.08 72.5,-81 65.39,-72.84 58.69,-63.07 53.15,-54.15"/>
<text text-anchor="start" x="72.5" y="-74.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">m_x</text>
<text text-anchor="start" x="32.15" y="-56.75" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="73.97" y="-92.34" font-family="Times New Roman,serif" font-size="8.00">2 m </text>
</g>
<!-- 7200964442915660904&#45;&#45;14420421744326312879 -->
<g id="edge12" class="edge">
<title>7200964442915660904&#45;&#45;14420421744326312879</title>
<path fill="none" stroke="black" d="M166.53,-99C179.88,-84.4 195.13,-67.75 207.49,-54.23"/>
<text text-anchor="start" x="189.5" y="-74.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">W_x</text>
<text text-anchor="start" x="186.49" y="-56.83" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="151.53" y="-92.6" font-family="Times New Roman,serif" font-size="8.00">3 w </text>
</g>
<!-- 12615399910196930301&#45;&#45;11769799603831642054 -->
<g id="edge15" class="edge">
<title>12615399910196930301&#45;&#45;11769799603831642054</title>
<path fill="none" stroke="black" d="M221.65,-332.87C209.23,-318.74 194.94,-302.46 182.51,-288.31"/>
<text text-anchor="start" x="205.5" y="-308.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">fx_k</text>
<text text-anchor="start" x="161.51" y="-290.91" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="205.65" y="-326.47" font-family="Times New Roman,serif" font-size="8.00">2 m </text>
</g>
<!-- 12615399910196930301&#45;&#45;16861794541815506586 -->
<g id="edge13" class="edge">
<title>12615399910196930301&#45;&#45;16861794541815506586</title>
<path fill="none" stroke="black" d="M280.3,-332.87C291.48,-318.74 304.36,-302.46 315.56,-288.31"/>
<text text-anchor="start" x="301.5" y="-308.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">Qi</text>
<text text-anchor="start" x="294.56" y="-290.91" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="265.3" y="-326.47" font-family="Times New Roman,serif" font-size="8.00">3 w </text>
</g>
<!-- 2184084146676702280&#45;&#45;18030336638603525491 -->
<g id="edge3" class="edge">
<title>2184084146676702280&#45;&#45;18030336638603525491</title>
<path fill="none" stroke="black" d="M172.74,-690.34C163.56,-683.42 154.93,-675.29 148.5,-666 143.01,-658.08 139.53,-648.38 137.31,-639.03"/>
<text text-anchor="start" x="148.5" y="-659.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">hx_k</text>
<text text-anchor="start" x="116.31" y="-641.63" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="156.74" y="-692.94" font-family="Times New Roman,serif" font-size="8.00">2 m </text>
</g>
<!-- 2184084146676702280&#45;&#45;4789214632737044419 -->
<g id="edge6" class="edge">
<title>2184084146676702280&#45;&#45;4789214632737044419</title>
<path fill="none" stroke="black" d="M255.52,-683.87C267.2,-669.74 280.64,-653.46 292.33,-639.31"/>
<text text-anchor="start" x="277.5" y="-659.6" font-family="Times New Roman,serif" font-size="8.00" fill="red">Ri</text>
<text text-anchor="start" x="271.33" y="-641.91" font-family="Times New Roman,serif" font-size="8.00">1 out </text>
<text text-anchor="start" x="240.52" y="-677.47" font-family="Times New Roman,serif" font-size="8.00">3 w </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, fx_k, hx_k, Qi, Ri, ids=[:xk, :fk, :hk, :Qi, :Ri])

# Define a variational message passing procedure
algorithm = messagePassingAlgorithm([x_k, fx_k, hx_k, 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
# Number of iterations
num_iterations = 5

# Parameters for prior distributions
m_x_0 = [1., 0.]
W_x_0 = [1. 0.;0. 1.]
a_R_0 = 10.0
b_R_0 = 0.1
V_Q_0 = [10. 0.1;0.1 1.]
n_Q_0 = 10.0

# Initialize array to track Free Energy objective
F = zeros(T,num_iterations)

# Initialize arrays for storing parameter estimates
m_x_k = zeros(2,T)
W_x_k = zeros(2,2,T)
a_R_k = zeros(T,)
b_R_k = zeros(T,)
V_Q_k = zeros(2,2,T)
n_Q_k = zeros(T,)

# Initialize previous parameter estimates
m_x_kmin1 = m_x_0
W_x_kmin1 = W_x_0
a_R_kmin1 = a_R_0
b_R_kmin1 = b_R_0
V_Q_kmin1 = V_Q_0
n_Q_kmin1 = n_Q_0

# Initialize data dictionary
data = Dict()

# Initialize marginal distributions
marginals = Dict()
marginals[:x_kmin1] = ProbabilityDistribution(Multivariate, GaussianMeanVariance, m=m_x_0, v=W_x_0)
marginals[:hx_k] = ProbabilityDistribution(Multivariate, GaussianMeanPrecision, m=[0., 0.], w=[1. 0.;0. 1.])
marginals[:fx_k] = ProbabilityDistribution(Multivariate, GaussianMeanVariance, m=[0., 0.], v=[1. 0.;0. 1.])
marginals[:x_k] = ProbabilityDistribution(Multivariate, GaussianMeanPrecision, m=[0., 0.], w=[1. 0.;0. 1.])
marginals[:x_1] = ProbabilityDistribution(Univariate, GaussianMeanVariance, m=0.0, v=1.0)
marginals[:Ri] = ProbabilityDistribution(Univariate, Gamma, a=a_R_0, b=b_R_0)
marginals[:Qi] = ProbabilityDistribution(Wishart, v=V_Q_0, nu=n_Q_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_x] = m_x_kmin1
    data[:W_x] = W_x_kmin1
    data[:a_R] = a_R_kmin1
    data[:b_R] = b_R_kmin1
    data[:V_Q] = V_Q_kmin1
    data[:n_Q] = n_Q_kmin1
    
    # Iteratively update beliefs
    for tt = 1:num_iterations
        
        # Update recognition distributions
        stepxk!(data, marginals)
        stepfk!(data, marginals)
        stephk!(data, marginals)
        stepRi!(data, marginals)
        stepQi!(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])
    a_R_kmin1 = a_R_k[k] = marginals[:Ri].params[:a]
    b_R_kmin1 = b_R_k[k] = marginals[:Ri].params[:b]
    V_Q_kmin1 = V_Q_k[:,:,k] = marginals[:Qi].params[:v]
    n_Q_kmin1 = n_Q_k[k] = marginals[:Qi].params[:nu]
    
end
```

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


### Inspect results
We can plot the estimates of the angle and the change in the angle, over time, including the uncertainty of the estimates.


```julia
# Plot true states and overlay estimates
plot((1:T).*Δt, states[1,:], linewidth=2, xlabel="time (Δt = "*string(Δt)*")", ylabel="angle (α)", color="blue", label="states", size=(800,400))
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")
title!("Angle of the pendulum, over time")
```


```julia
# Plot true states and overlay estimates
plot((1:T).*Δt, states[2,:], linewidth=2, xlabel="time (Δt = "*string(Δt)*")", ylabel="change in angle (dα/dt)", color="blue", label="states", size=(800,400))
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", legend=:bottomright)
title!("Change in angle of the pendulum, over time")
```

Both the angle and the change in angle are tracked nicely.

a1079e44dfed254cc545a9d4e8050de1efdb64db
ipynb
Jupyter Notebook
demos/Noisy pendulum.ipynb
wmkouw/nonlin-filters
a1ea875ab5a41352c9ab2a5dcfa1c65e1a0f632e
MIT
demos/Noisy pendulum.ipynb
wmkouw/nonlin-filters
a1ea875ab5a41352c9ab2a5dcfa1c65e1a0f632e
MIT
demos/Noisy pendulum.ipynb
wmkouw/nonlin-filters
a1ea875ab5a41352c9ab2a5dcfa1c65e1a0f632e
MIT
Qwen/Qwen-72B
1. YES
2. YES