@@ -50,7 +50,7 @@ is good practice to first simulate `model` using [`sim!`](@ref) as a quick sanit
5050
5151``` @example 1
5252using Plots
53- u = [0.5] # τ = 0.5 N m
53+ u = [0.5]
5454plot(sim!(model, 60, u), plotu=false)
5555```
5656
@@ -59,7 +59,7 @@ plot(sim!(model, 60, u), plotu=false)
5959An [ ` UnscentedKalmanFilter ` ] ( @ref ) estimates the plant state :
6060
6161``` @example 1
62- estim = UnscentedKalmanFilter(model, σQ=[0.5, 2.5], σQ_int=[0.5])
62+ estim = UnscentedKalmanFilter(model, σQ=[0.5, 2.5], σQ_int=[5.0], σR=[ 0.5])
6363```
6464
6565The standard deviation of the angular velocity `` ω `` is higher here (` σQ ` second value)
@@ -70,24 +70,33 @@ since ``\dot{ω}(t)`` equation includes an uncertain parameter: the friction coe
7070par_plant = (par[1], par[2], par[3] + 0.25, par[4])
7171f_plant(x, u, _) = x + Ts*pendulum(par_plant, x, u)
7272plant = NonLinModel(f_plant, h, Ts, nu, nx, ny)
73- res = sim!(estim, 30 , [0.5], plant=plant, y_noise=[0.5]) # τ = 0.5 N m
74- plot(res, plotu=false, plotx=true, plotx̂ =true)
73+ res = sim!(estim, 60 , [0.5], plant=plant, y_noise=[0.5])
74+ plot(res, plotu=false, plotxwithx̂ =true)
7575```
7676
77- The Kalman filter performance seems sufficient for control. As the motor torque is limited
78- to -1.5 to 1.5 N m, we incorporate the input constraints in a [ ` NonLinMPC ` ] ( @ref ) :
77+ The estimate `` x̂_3 `` is the integrator state that compensates for static errors (` nint_ym `
78+ parameter of [ ` UnscentedKalmanFilter ` ] ( @ref ) ). The Kalman filter performance seems
79+ sufficient for control. As the motor torque is limited to -1.5 to 1.5 N m, we incorporate
80+ the input constraints in a [ ` NonLinMPC ` ] ( @ref ) :
7981
8082``` @example 1
81- mpc = NonLinMPC(estim, Hp=20, Hc=2 , Mwt=[0.1], Nwt=[1.0], Cwt=Inf)
83+ mpc = NonLinMPC(estim, Hp=20, Hc=4 , Mwt=[0.1], Nwt=[1.0], Cwt=Inf)
8284mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5])
8385```
8486
8587We test ` mpc ` performance on ` plant ` by imposing an angular setpoint of 180° (inverted
8688position):
8789
8890``` @example 1
89- res = sim!(mpc, 30 , [180.0], x̂0=zeros(mpc.estim.nx̂), plant=plant, x0=zeros(plant.nx))
90- plot(res, plotŷ=true )
91+ res = sim!(mpc, 65 , [180.0], plant=plant, x0=zeros(plant.nx), x̂0=zeros(mpc.estim.nx̂ ))
92+ plot(res)
9193```
9294
93- The controller seems robust enough to variations on `` K `` coefficient.
95+ The controller seems robust enough to variations on `` K `` coefficient. Moreover, starting
96+ from this inverted position, the closed-loop response to a step disturbances of 10° on `` θ ``
97+ is also satisfactory:
98+
99+ ``` @example 1
100+ res = sim!(mpc, 65, [180.0], plant=plant, x0=[π, 0], x̂0=[π, 0, 0], y_step=[10])
101+ plot(res)
102+ ```
0 commit comments