Algorithm 1. Algorithm for a nonlinear MHE

Set $k=0$ $\left(k\in {\mathbb{N}}^{+}\right)$ and set ${\mathit{x}}_k={\mathit{x}}_0$

while (brake==false) do

Fill ring buffer with measurements and system inputs:

if $k<M$ then

append ${\mathit{u}}_k$ to $\mathit{u}$ and ${\mathit{y}}_k^{\mathrm{m}}$ to ${\mathit{y}}^{\mathrm{m}}$

else

left shift one entry of $\mathit{u}$ and ${\mathit{y}}^{\mathrm{m}}$

and append ${\mathit{u}}_k$ respectively ${\mathit{y}}_k^{\mathrm{m}}$

end if

Optimize over stored measurement window (Equation (8)):

if $k>M$ then

Propagate ${\widehat{\mathit{x}}}_{k-M-1}^{+}$ via a Kalman Filter step (e.g., EKF or UKF cf. 2):

xKF(${\widehat{\mathit{x}}}_{k-M-1}^{+})\to {\widehat{\mathit{x}}}_{k-M}^{+}, {\mathit{I}}_{k-M}^{+}$

Project states on the constrained area (c.f. Appendix B)

$\underset{\mathit{x}}{\min }\Vert \mathit{x}-{\widehat{\mathit{x}}}_{k-M}^{+}\Vert \mathrm{s}.\mathrm{t}. \mathit{c}\left(\mathit{x}\right)\le 0$

end if

$k=k+1$

end while