Algorithm 1 Variational measurement update.

Input: $\left\{{\hat{\mathit{x}}}_{k|k-1},{\mathit{P}}_{k|k-1}\right\},{\left\{{\mathit{\alpha }}_{k|k-1}^i,{\mathit{\beta }}_{k|k-1}^i\right\}}_{i=1}^{n_y},\left\{{\hat{\theta }}_{k|k-1},{\Theta }_{k|k-1}\right\},{\mathit{Z}}_k$

Output: ${\hat{\mathit{x}}}_{k|k},{\mathit{P}}_{k|k},{\left\{{\mathit{\alpha }}_{k|k}^i,{\mathit{\beta }}_{k|k}^i\right\}}_{i=1}^{n_y},{\hat{\theta }}_{k|k},{\Theta }_{k|k}$

Initialization:

${\hat{\mathit{x}}}_{0,k|k}^{\left(0\right)}\leftarrow {\hat{\mathit{x}}}_{k|k-1}, {\mathit{P}}_{0,k|k}^{\left(0\right)}\leftarrow {\mathit{P}}_{k|k-1}, {\hat{\theta }}_{0,k|k}^{\left(0\right)}\leftarrow {\hat{\theta }}_{k|k-1}, {\Theta }_{0,k|k}^{\left(0\right)}\leftarrow {\Theta }_{k|k-1},$

${\mathit{\alpha }}_{0,k|k}^{i,\left(0\right)}\leftarrow {\mathit{\alpha }}_{k|k-1}^i,{\mathit{\beta }}_{0,k|k}^{i,\left(0\right)}\leftarrow {\mathit{\beta }}_{k|k-1}^i\mathit{for} i=1,\dots ,n_y,$

${\mathit{y}}_k^{j,\left(0\right)}\leftarrow {\mathit{z}}_k^j \mathit{for} j=1,\dots n_k,$

${\mathit{\Sigma }}_k^{y,\left(0\right)}\leftarrow {\mathbb{E}}_{q_X^{\left(0\right)}}\left[\left(s{\mathit{X}}_k\right)\right] \mathit{using}\left(30\right)$

Iterations:

for  $n=0,\dots ,n_{max}-1$

Calculate $R({\hat{\mathit{x}}}_{n+1,k+1|k})$ using (9)

for  $\ell =0,\dots ,{\ell }_{max}-1$

Calculate $\left\{{\hat{\mathit{x}}}_{n+1,k|k}^{\left(\ell +1\right)},{\mathit{P}}_{n+1,k|k}^{\left(\ell +1\right)}\right\}$ using (24)   Calculate ${\left\{{\mathit{\alpha }}_{n+1,k|k}^{i,\left(\ell +1\right)},{\mathit{\beta }}_{n+1,k|k}^{i,\left(\ell +1\right)}\right\}}_{i=1}^{n_y}$ using (26)   Calculate ${\left\{{\hat{\mathit{y}}}_{n+1,k|k}^{\left(\ell +1\right)},{\mathit{\Sigma }}_{n+1,k|k}^{\left(\ell +1\right)}\right\}}_{i=1}^{n_k}$ using (29)   Calculate $\left\{{\hat{\theta }}_{n+1,k|k}^{\left(\ell +1\right)},{\Theta }_{n+1,k|k}^{\left(\ell +1\right)}\right\}$ using (32)

end for

${\hat{\mathit{x}}}_{n+1,k|k-1}^{\left(0\right)}\leftarrow {\hat{\mathit{x}}}_{n,k|k}{\mathit{P}}_{n+1,k|k-1}^{\left(0\right)}\leftarrow {\mathit{P}}_{n,k|k}, {\hat{\theta }}_{n+1,k|k-1}^{\left(0\right)}\leftarrow {\hat{\theta }}_{n,k|k}, {\Theta }_{n+1,k|k-1}^{\left(0\right)}\leftarrow {\Theta }_{n,k|k},$

${\mathit{\alpha }}_{n+1,k|k-1}^{i,\left(0\right)}\leftarrow {\mathit{\alpha }}_{n,k|k}^i, {\mathit{\beta }}_{n+1,k|k-1}^{i,\left(0\right)}\leftarrow {\mathit{\beta }}_{n,k|k}^i \mathit{for} i=1,\dots ,n_y,$

${\mathit{y}}_{n+1,k|k}^{j,\left(0\right)}\leftarrow {\mathit{z}}_k^j \mathit{for} j=1,\dots n_k,$   ${\mathit{\Sigma }}_{n+1,k}^{y,\left(0\right)}\leftarrow {\mathit{\Sigma }}_k^{y,\left(0\right)}$

end for