Nonlinear Constrained Moving Horizon Estimation Applied to Vehicle Position Estimation

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

Algorithm A1. Simplified Newton descent search algorithm for constrained estimation projection.

1. Set

μ_{o} = 0

and

k = - 1

and iter=1 and max_iter=30

Set

α = {(\nabla c {({\hat{x}}_{k}^{+})}^{T} \cdot \nabla c ({\hat{x}}_{k}^{+}))}^{- 1}

while

| α \cdot c ({\hat{x}}_{k}^{+} + μ_{k + 1} \cdot \nabla c ({\hat{x}}_{k}^{+})) | \geq ϵ_{Newton}

and iter < max_iter do

k = k + 1

Δ μ_{k + 1} = - α \cdot c ({\hat{x}}_{k}^{+} + μ_{k} \cdot \nabla c ({\hat{x}}_{k}^{+}))

μ_{k + 1} = μ_{k} + Δ μ_{k + 1}

i t e r = i t e r + 1

end while

Solution:

{\hat{x}}_{k}^{P} = {\hat{x}}_{k}^{+} + μ_{k + 1} \cdot \nabla c ({\hat{x}}_{k}^{+})