From efd3d58d5c3e18b503f717f7a65ca18bd5d61d84 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 25 Jun 2024 15:04:32 -0400 Subject: [PATCH 1/2] doc: SLMPC first linearization at resting position --- docs/src/manual/nonlinmpc.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index df94a0d2d..a1256853b 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/docs/src/manual/nonlinmpc.md @@ -338,10 +338,12 @@ the [`LinMPC`](@ref) instance based on repeated online linearization. The [`setmodel!`](@ref) method allows online adaptation of a linear plant model. Combined with the automatic linearization of [`linearize`](@ref), a successive linearization MPC can be designed with minimal efforts. The [`SteadyKalmanFilter`](@ref) does not support -[`setmodel!`](@ref), so we need to use the time-varying [`KalmanFilter`](@ref) instead: +[`setmodel!`](@ref) so we need to use the time-varying [`KalmanFilter`](@ref), and we +initialize it with a linearization at ``θ = ω = τ = 0``: ```@example 1 -kf = KalmanFilter(linmodel; σQ, σR, nint_u, σQint_u) +linmodel = linearize(model, x=[0, 0], u=[0]) +kf = KalmanFilter(linmodel; σQ, σR, nint_u, σQint_u) mpc3 = LinMPC(kf; Hp, Hc, Mwt, Nwt, Cwt=Inf, optim=daqp) mpc3 = setconstraint!(mpc3; umin, umax) ``` From 5548ac9bb36a4202c520d1d00d53f1f42c24395c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 25 Jun 2024 16:34:04 -0400 Subject: [PATCH 2/2] doc: minor modifications in slmpc simulation function --- docs/src/manual/nonlinmpc.md | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index a1256853b..9d343a01f 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/docs/src/manual/nonlinmpc.md @@ -351,14 +351,14 @@ mpc3 = setconstraint!(mpc3; umin, umax) We create a function that simulates the plant and the adaptive controller: ```@example 1 -function test_slmpc(nonlinmodel, mpc, ry, plant; x_0=plant.xop, y_step=0) - N = 35 +function sim_adapt!(mpc, nonlinmodel, N, ry, plant, x_0, x̂_0, y_step=[0]) U_data, Y_data, Ry_data = zeros(plant.nu, N), zeros(plant.ny, N), zeros(plant.ny, N) setstate!(plant, x_0) - u, y = [0.0], plant() - x̂ = initstate!(mpc, u, y) + initstate!(mpc, [0], plant()) + setstate!(mpc, x̂_0) + x̂ = x̂_0 for i = 1:N - y = plant() .+ y_step + y = plant() + y_step u = moveinput!(mpc, ry) linmodel = linearize(nonlinmodel; u, x=x̂[1:2]) setmodel!(mpc, linmodel) @@ -378,7 +378,8 @@ The [`SimResult`](@ref) object is for plotting purposes only. The adaptive [`Lin performances are similar to the nonlinear MPC, both for the 180° setpoint: ```@example 1 -res_slin = test_slmpc(model, mpc3, [180], plant, x_0=[0, 0]) +x_0 = [0, 0]; x̂_0 = [0, 0, 0]; ry = [180] +res_slin = sim_adapt!(mpc3, model, N, ry, plant, x_0, x̂_0) plot(res_slin) savefig("plot10_NonLinMPC.svg"); nothing # hide ``` @@ -388,7 +389,8 @@ savefig("plot10_NonLinMPC.svg"); nothing # hide and the 10° step disturbance: ```@example 1 -res_slin = test_slmpc(model, mpc3, [180], plant, x_0=[π, 0], y_step=[10]) +x_0 = [π, 0]; x̂_0 = [π, 0, 0]; y_step = [10] +res_slin = sim_adapt!(mpc3, model, N, ry, plant, x_0, x̂_0, y_step) plot(res_slin) savefig("plot11_NonLinMPC.svg"); nothing # hide ```