Table - PMC

Skip to main content

View full-text article in PMC

. 2019 May 16;19(10):2276. doi: 10.3390/s19102276

Algorithm 4. Multiple shooting MHE gradient calculation

Define:

R^{*} = {(R \cdot R)}^{- 1}; Q^{*} = {(Q \cdot Q)}^{- 1}

;

E = eye (n)

Define:

i_{1} (i) : = 1 + (i - k + M) n : (i - k + M + 1) n

\nabla g_{1 : n} = (I_{k - M}^{+} + {(I_{k - M}^{+})}^{T}) (x_{k - M} - {\hat{x}}_{k - M}^{+}) - 2 {\frac{\partial h (x_{k - M})}{\partial x_{k - M}}}^{T} R^{*} (y_{k - M}^{m} - h (x_{k - M}))

for

i = k - M + 1

to

k

do

\nabla g_{i_{1} (i)} = \frac{\partial g (ξ_{k})}{\partial x_{i}} = 2 Q^{*} (x_{i} - x_{int, i}) - 2 \frac{\partial h {(x_{i})}^{T}}{\partial x_{i}} R^{*} (y_{i}^{m} - h (x_{i}))

if version == V3 then

\nabla g_{i_{1} (i)} - = {(\frac{1}{2} \cdot T_{s} \cdot \frac{\partial f_{i}}{\partial x_{i}})}^{T} \cdot 2 \cdot Q_{MS} \cdot (x_{i} - x_{int, i}^{L})

end if

end for

Define:

j_{1} (j) : = 1 + (j - k + M) n : (j - k + M + 1) n

for

j = k - M

to

k - 1

do

\nabla g_{j_{1} (j)} + = \frac{\partial g (ξ_{k})}{\partial x_{j}}

switch(version)

case V1:

\nabla g_{j_{1} (j)} + = - \frac{\partial f_{j}}{\partial x_{j}} \cdot 2 \cdot Q_{MS} \cdot (x_{j + 1} - x_{int, j + 1}^{L})

case V2:

\nabla g_{j_{1} (j)} + = - {(\frac{1}{2} \cdot T_{s} \cdot \frac{\partial f_{j}}{\partial x_{j}})}^{T} \cdot 2 \cdot Q_{MS} \cdot (x_{j + 1} - x_{int, j + 1}^{L})

case V3:

\nabla g_{j_{1} (j)} - = {(E + \frac{1}{2} \cdot T_{s} \cdot \frac{\partial f_{j}}{\partial x_{j}})}^{T} \cdot 2 \cdot Q_{MS} \cdot (x_{j + 1} - x_{int, j + 1}^{L})

end switch

end for