Skip to content

Commit 3415b55

Browse files
franckgagafranckgaga
committed
added: compute PuDagger matrix for custom move blocking
1 parent 0a442d0 commit 3415b55

File tree

5 files changed

+55
-40
lines changed

5 files changed

+55
-40
lines changed

‎src/controller/construct.jl

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -458,7 +458,7 @@ estimate_delays(::SimModel) = 0
458458

459459

460460
"""
461-
move_blocking(Hp::Int, Hc::AbstractVector{Int}) -> nb, Hc
461+
move_blocking(Hp::Int, Hc::AbstractVector{Int}) -> nb
462462
463463
Get move blocking vector `nb` and actual control horizon from `Hp` and `Hc` arguments.
464464
@@ -483,12 +483,11 @@ function move_blocking(Hp_arg::Int, Hc_arg::AbstractVector{Int})
483483
nb[end] = Hp - @views sum(nb[begin:end-1])
484484
end
485485
end
486-
Hc = length(nb)
487-
return nb, Hc
486+
return nb
488487
end
489488

490489
"""
491-
move_blocking(Hp::Int, Hc::Int) -> nb, Hc
490+
move_blocking(Hp::Int, Hc::Int) -> nb
492491
493492
Construct a move blocking vector `nb` that match the provided `Hp` and `Hc` horizons.
494493
@@ -498,9 +497,12 @@ function move_blocking(Hp_arg::Int, Hc_arg::Int)
498497
Hp, Hc = Hp_arg, Hc_arg
499498
nb = fill(1, Hc)
500499
nb[end] = Hp - Hc + 1
501-
return nb, Hc
500+
return nb
502501
end
503502

503+
"Get the actual control Horizon `Hc` (integer) from the move blocking vector `nb`."
504+
get_Hc(nb::AbstractVector{Int}) = length(nb)
505+
504506

505507
"""
506508
validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u)

‎src/controller/explicitmpc.jl

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -135,7 +135,7 @@ function ExplicitMPC(
135135
Nwt = fill(DEFAULT_NWT, model.nu),
136136
Lwt = fill(DEFAULT_LWT, model.nu),
137137
M_Hp = Diagonal(repeat(Mwt, Hp)),
138-
N_Hc = Diagonal(repeat(Nwt, Hc)),
138+
N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
139139
L_Hp = Diagonal(repeat(Lwt, Hp)),
140140
kwargs...
141141
)
@@ -158,7 +158,7 @@ function ExplicitMPC(
158158
Nwt = fill(DEFAULT_NWT, estim.model.nu),
159159
Lwt = fill(DEFAULT_LWT, estim.model.nu),
160160
M_Hp = Diagonal(repeat(Mwt, Hp)),
161-
N_Hc = Diagonal(repeat(Nwt, Hc)),
161+
N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
162162
L_Hp = Diagonal(repeat(Lwt, Hp)),
163163
) where {NT<:Real, SE<:StateEstimator{NT}}
164164
isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR)
@@ -167,7 +167,8 @@ function ExplicitMPC(
167167
@warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
168168
"($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
169169
end
170-
nb, Hc = move_blocking(Hp, Hc)
170+
nb = move_blocking(Hp, Hc)
171+
Hc = get_Hc(nb)
171172
weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp)
172173
return ExplicitMPC{NT}(estim, Hp, Hc, nb, weights)
173174
end

‎src/controller/linmpc.jl

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -211,7 +211,7 @@ function LinMPC(
211211
Nwt = fill(DEFAULT_NWT, model.nu),
212212
Lwt = fill(DEFAULT_LWT, model.nu),
213213
M_Hp = Diagonal(repeat(Mwt, Hp)),
214-
N_Hc = Diagonal(repeat(Nwt, Hc)),
214+
N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
215215
L_Hp = Diagonal(repeat(Lwt, Hp)),
216216
Cwt = DEFAULT_CWT,
217217
transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION,
@@ -255,7 +255,7 @@ function LinMPC(
255255
Nwt = fill(DEFAULT_NWT, estim.model.nu),
256256
Lwt = fill(DEFAULT_LWT, estim.model.nu),
257257
M_Hp = Diagonal(repeat(Mwt, Hp)),
258-
N_Hc = Diagonal(repeat(Nwt, Hc)),
258+
N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
259259
L_Hp = Diagonal(repeat(Lwt, Hp)),
260260
Cwt = DEFAULT_CWT,
261261
transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION,
@@ -267,7 +267,8 @@ function LinMPC(
267267
@warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
268268
"($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
269269
end
270-
nb, Hc = move_blocking(Hp, Hc)
270+
nb = move_blocking(Hp, Hc)
271+
Hc = get_Hc(nb)
271272
weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
272273
return LinMPC{NT}(estim, Hp, Hc, nb, weights, transcription, optim)
273274
end

‎src/controller/nonlinmpc.jl

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -287,7 +287,7 @@ function NonLinMPC(
287287
Nwt = fill(DEFAULT_NWT, model.nu),
288288
Lwt = fill(DEFAULT_LWT, model.nu),
289289
M_Hp = Diagonal(repeat(Mwt, Hp)),
290-
N_Hc = Diagonal(repeat(Nwt, Hc)),
290+
N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
291291
L_Hp = Diagonal(repeat(Lwt, Hp)),
292292
Cwt = DEFAULT_CWT,
293293
Ewt = DEFAULT_EWT,
@@ -344,7 +344,7 @@ function NonLinMPC(
344344
Nwt = fill(DEFAULT_NWT, estim.model.nu),
345345
Lwt = fill(DEFAULT_LWT, estim.model.nu),
346346
M_Hp = Diagonal(repeat(Mwt, Hp)),
347-
N_Hc = Diagonal(repeat(Nwt, Hc)),
347+
N_Hc = Diagonal(repeat(Nwt, get_Hc(move_blocking(Hp, Hc)))),
348348
L_Hp = Diagonal(repeat(Lwt, Hp)),
349349
Cwt = DEFAULT_CWT,
350350
Ewt = DEFAULT_EWT,
@@ -366,7 +366,8 @@ function NonLinMPC(
366366
@warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
367367
"($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
368368
end
369-
nb, Hc = move_blocking(Hp, Hc)
369+
nb = move_blocking(Hp, Hc)
370+
Hc = get_Hc(nb)
370371
validate_JE(NT, JE)
371372
gc! = get_mutating_gc(NT, gc)
372373
weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)

‎src/controller/transcription.jl

Lines changed: 36 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -134,29 +134,32 @@ The ``\mathbf{P_u}`` and ``\mathbf{T_u}`` matrices are defined in the Extended H
134134
135135
# Extended Help
136136
!!! details "Extended Help"
137+
With ``n_j``, the ``j``th element of the ``\mathbf{n_b}`` vector introduced in [`init_ZtoΔU`](@ref)
138+
documentation, we introduce the ``\mathbf{Q}(j)`` matrix of size `(nu*nj, nu)`:
139+
```math
140+
\mathbf{Q}(j) = \begin{bmatrix}
141+
\mathbf{I} \\
142+
\mathbf{I} \\
143+
\vdots \\
144+
\mathbf{I} \end{bmatrix}
145+
```
137146
The ``\mathbf{U}`` vector and the conversion matrices are defined as:
138147
```math
139148
\mathbf{U} = \begin{bmatrix}
140-
\mathbf{u}(k + 0) \\
141-
\mathbf{u}(k + 1) \\
142-
\vdots \\
143-
\mathbf{u}(k + H_c - 1) \\
144-
\vdots \\
145-
\mathbf{u}(k + H_p - 1) \end{bmatrix} , \quad
149+
\mathbf{u}(k + 0) \\
150+
\mathbf{u}(k + 1) \\
151+
\vdots \\
152+
\mathbf{u}(k + H_p - 1) \end{bmatrix} , \quad
146153
\mathbf{P_u^†} = \begin{bmatrix}
147-
\mathbf{I} & \mathbf{0} & \cdots & \mathbf{0} \\
148-
\mathbf{I} & \mathbf{I} & \cdots & \mathbf{0} \\
149-
\vdots & \vdots & \ddots & \vdots \\
150-
\mathbf{I} & \mathbf{I} & \cdots & \mathbf{I} \\
151-
\vdots & \vdots & \ddots & \vdots \\
152-
\mathbf{I} & \mathbf{I} & \cdots & \mathbf{I} \end{bmatrix} , \quad
154+
\mathbf{Q}(n_1) & \mathbf{0} & \cdots & \mathbf{0} \\
155+
\mathbf{Q}(n_2) & \mathbf{Q}(n_2) & \cdots & \mathbf{0} \\
156+
\vdots & \vdots & \ddots & \vdots \\
157+
\mathbf{Q}(n_{H_c}) & \mathbf{Q}(n_{H_c}) & \cdots & \mathbf{Q}(n_{H_c}) \end{bmatrix} , \quad
153158
\mathbf{T_u} = \begin{bmatrix}
154-
\mathbf{I} \\
155-
\mathbf{I} \\
156-
\vdots \\
157-
\mathbf{I} \\
158-
\vdots \\
159-
\mathbf{I} \end{bmatrix}
159+
\mathbf{I} \\
160+
\mathbf{I} \\
161+
\vdots \\
162+
\mathbf{I} \end{bmatrix}
160163
```
161164
and, depending on the transcription method, we have:
162165
- ``\mathbf{P_u} = \mathbf{P_u^†}`` if `transcription` is a [`SingleShooting`](@ref)
@@ -166,19 +169,26 @@ The ``\mathbf{P_u}`` and ``\mathbf{T_u}`` matrices are defined in the Extended H
166169
function init_ZtoU(
167170
estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc, nb
168171
) where {NT<:Real}
169-
model = estim.model
172+
nu = estim.model.nu
170173
# Pu and Tu are `Matrix{NT}`, conversion is faster than `Matrix{Bool}` or `BitMatrix`
171-
I_nu = Matrix{NT}(I, model.nu, model.nu)
172-
PU_Hc = LowerTriangular(repeat(I_nu, Hc, Hc))
173-
PUdagger = [PU_Hc; repeat(I_nu, Hp - Hc, Hc)]
174-
Pu = init_PUmat(estim, transcription, Hp, Hc, PUdagger)
174+
I_nu = Matrix{NT}(I, nu, nu)
175+
PuDagger = Matrix{NT}(undef, nu*Hp, nu*Hc)
176+
for j=1:Hc
177+
nj = nb[j]
178+
Qj = repeat(I_nu, nj, 1)
179+
iRows = (1:nu*nj) .+ @views nu*sum(nb[1:j-1])
180+
PuDagger[iRows, :] = [repeat(Qj, 1, j) zeros(nu*nj, nu*(Hc-j))]
181+
end
182+
Pu = init_PUmat(estim, transcription, Hp, Hc, PuDagger)
175183
Tu = repeat(I_nu, Hp)
176184
return Pu, Tu
177185
end
178186

179-
init_PUmat( _ , ::SingleShooting, _ , _ , PUdagger) = PUdagger
180-
function init_PUmat(estim, ::MultipleShooting, Hp, _ , PUdagger)
181-
return [PUdagger zeros(eltype(PUdagger), estim.model.nu*Hp, estim.nx̂*Hp)]
187+
188+
189+
init_PUmat( _ , ::SingleShooting, _ , _ , PuDagger) = PuDagger
190+
function init_PUmat(estim, ::MultipleShooting, Hp, _ , PuDagger)
191+
return [PuDagger zeros(eltype(PuDagger), estim.model.nu*Hp, estim.nx̂*Hp)]
182192
end
183193

184194
@doc raw"""

0 commit comments

Comments
 (0)