Algorithm 3. MHE calculation by means of functional mockup unit (FMU) evaluations

Define: ${\mathit{R}}^{*}={\left(\mathit{R}\cdot {\mathit{R}}^T\right)}^{-1}$ and ${\mathit{Q}}^{*}={\left(\mathit{Q}\cdot {\mathit{Q}}^T\right)}^{-1}$

$\mathit{\nabla }{g}_{1:n}=\left({\mathit{I}}_{k-M}^{+}+{({\mathit{I}}_{k-M}^{+})}^T\right)\left({\mathit{x}}_{k-M}-{\widehat{\mathit{x}}}_{k-M}^{+}\right) -2{\frac{\partial h\left({\mathit{x}}_{k-M}\right)}{\partial {\mathit{x}}_{k-M}}}^T{\mathit{R}}^{*}\left({\mathit{y}}_{k-M}^{\mathrm{m}}-\mathit{h}\left({\mathit{x}}_{k-M}\right)\right)$

for$i=k-M+1 \mathrm{to} k$do

$\mathbf{\nabla }{g}_{1+\left(i-k+M\right)\cdot n:\left(i-k+M+1\right)\cdot n}=\frac{\partial g\left({\mathit{\xi }}_k\right)}{\partial {\mathit{x}}_i}=2{\mathit{Q}}^{*}\left({\mathit{x}}_i-{\mathit{x}}_{\mathrm{int},i}\right)-2\frac{\partial \mathit{h}{\left({\mathit{x}}_i\right)}^T}{\partial {\mathit{x}}_i}{\mathit{R}}^{*}\left({\mathit{y}}_i^{\mathrm{m}}-\mathit{h}\left({\mathit{x}}_i\right)\right)$

end for