Multiple shooting transcription for `MovingHorizonEstimator`

[franckgaga]

Right now, everything is hard-coded as a single shooting transcription in the MHE. It leads to Jacobian and Hessian matrices that are "very denses", surprisingly way more than NonLinMPC with single shooting. For example, on the inverted pendulum:

using ModelPredictiveControl, JuMP, SparseArrays
function f!(ẋ, x, u, _ , p)
    g, L, K, m = p          # [m/s²], [m], [kg/s], [kg]
    θ, ω = x[1], x[2]       # [rad], [rad/s]
    τ  = u[1]               # [Nm]
    ẋ[1] = ω
    ẋ[2] = -g/L*sin(θ) - K/m*ω + τ/m/L^2
    return nothing
end
h!(y, x, _ , _ ) = (y[1] = 180/π*x[1]; nothing) # [°]
p = [9.8, 0.4, 1.2, 0.3]
nu, nx, ny, Ts = 1, 2, 1, 0.1
vu, vx, vy = ["\$τ\$ (Nm)"], ["\$θ\$ (rad)", "\$ω\$ (rad/s)"], ["\$θ\$ (°)"]
model = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny; p); u=vu, x=vx, y=vy)
p_plant = copy(p); p_plant[3] = p[3]*1.25
plant = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny; p=p_plant); u=vu, x=vx, y=vy)
N = 35
mhe = MovingHorizonEstimator(model, He=10, nint_ym=0, hessian=true)
unset_time_limit_sec(mhe.optim)
mhe = setconstraint!(mhe, v̂min=[-1], v̂max=[1])
sim!(mhe, N, [1.0]; plant=plant, x_0=[0, 0], x̂_0=[0, 0])
info = getinfo(mhe)
info[:∇²J] |> sparse |> display
info[:∇g] |> sparse |> display
info[:∇²ℓg] |> sparse |> display

printing:

⎡⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡇⎤
⎢⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡇⎥
⎢⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡇⎥
⎢⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡇⎥
⎢⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡇⎥
⎣⠉⠉⠉⠉⠉⠉⠉⠉⠉⠉⠑⎦

⎡⣿⣵⢄⠀⠀⠀⠀⠀⠀⠀⠀⎤
⎢⣿⣿⣿⣵⢄⠀⠀⠀⠀⠀⠀⎥
⎢⣿⣿⣿⣿⣿⣵⢄⠀⠀⠀⠀⎥
⎢⣿⣿⣿⣿⣿⣿⣿⣵⢄⠀⠀⎥
⎢⣿⣿⣿⣿⣿⣿⣿⣿⣿⣵⢄⎥
⎢⣿⣵⢄⠀⠀⠀⠀⠀⠀⠀⠀⎥
⎢⣿⣿⣿⣵⢄⠀⠀⠀⠀⠀⠀⎥
⎢⣿⣿⣿⣿⣿⣵⢄⠀⠀⠀⠀⎥
⎢⣿⣿⣿⣿⣿⣿⣿⣵⢄⠀⠀⎥
⎢⣿⣿⣿⣿⣿⣿⣿⣿⣿⣵⢄⎥
⎢⣿⣷⣦⣄⡀⠀⠀⠀⠀⠀⠀⎥
⎢⣿⣿⣿⣿⣿⣷⣦⣄⡀⠀⠀⎥
⎢⣿⣟⡛⠛⠛⠛⠛⠛⠛⠓⠂⎥
⎢⣿⣿⣿⣷⣦⣄⡀⠀⠀⠀⠀⎥
⎣⣿⣿⣿⣿⣿⣿⣿⣷⣦⣄⡀⎦

⎡⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⎤
⎢⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⎥
⎢⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⎥
⎢⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⎥
⎢⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⎥
⎣⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⎦

I verified, and yes, all dots are truly non-zero values! This may be related to this statement:

Due to the fact that the MHE problem has the same optimal control structure
as the NMPC problem, they are often called “dual” to each other, in a slight
abuse of terminology. However, the starkest contrast to the NMPC problem is
the fact that the MHE problem has a free initial value $x_0$ and often has a much
higher dimensional “control vector” $w_k$.

Considering all of this, a multiple shooting transcription may improve the performances of the MHE.