Setting `move_blocks` to all ones changes result

[baggepinnen]

I had expected that setting move_blocks to all ones, of length equal to prediction horizon, would be the same as having no move blocks, but doing so changes the result dramatically, and the constraints are no longer respected

ẍM = 50    # Maximum acceleration
ẋM = 10    # Maximum velocity
x⃛M = 8e3
Ts = 0.005 # Sample time
r(t) = 2.5 + 3 * (t - floor(t)) # Reference to be smoothed
t = 0:Ts:3-Ts # Time vector
R = r.(t) 
Rmat = [R 0*R 0*R]'

Ac = [0.0 1.0 0.0; 0.0 0.0 1.0; 0.0 0.0 0.0]
Bc = [0.0; 0.0; 1.0]

Cc = [1.0 0 0; 0 1 0; 0 0 1]
mpc = LinearMPC.MPC(Ac, Bc, Float64(Ts); C=Cc, Np=50, Nc=50)

set_objective!(mpc; Q=[1e4, 0, 0], R=[1e-6], Rr=[1e-4], Qf=[1e8, 1e-2, 1e-3])
set_input_bounds!(mpc; umin=[-x⃛M], umax=[x⃛M])
add_constraint!(mpc;
    Ax=[0.0 1.0 0.0],
    lb=[-ẋM], ub=[ẋM],
    soft=false)
add_constraint!(mpc;
    Ax=[0.0 0.0 1.0],
    lb=[-ẍM], ub=[ẍM],
    soft=false)

LinearMPC.set_prestabilizing_feedback!(mpc)
mpc.settings.reference_preview = true

setup!(mpc)

sim = LinearMPC.Simulation(mpc; r=Rmat, N=600)
plot(sim)

LinearMPC.move_block!(mpc,ones(50)) # Should make no difference?
sim = LinearMPC.Simulation(mpc; r=Rmat, N=600)
plot!(sim, c=2)

hline!([ẋM, -ẋM], sp=2, lab="", l=:dash)
hline!([ẍM, -ẍM], sp=3, lab="", l=:dash)
hline!([x⃛M, -x⃛M], sp=4, lab="", l=:dash)

[darnstrom]

This was related to #68. Should be fixed in #70.

[darnstrom]

Note that I added support for individual move blocks for each control input in #64, which required some refactoring of the move block logic. So there might be some bugs related to that which I was not able to catch yet. Thanks for catching this and #68 though!

[baggepinnen]

If you comment out the LinearMPC.set_prestabilizing_feedback!(mpc) in the example in the OP above I get different results also on latest master after #70

[darnstrom]

Strange, I cannot reproduce this with the code on main with the example you have above with LinearMPC.set_prestabilizing_feedback!(mpc) commented out . I get that they are identical (norm(sim1.xs-sim2.xs)≈5e-10.)

Specifically I run this code:

using LinearMPC,Plots
ẍM = 50    # Maximum acceleration
ẋM = 10    # Maximum velocity
x⃛M = 8e3
Ts = 0.005 # Sample time
r(t) = 2.5 + 3 * (t - floor(t)) # Reference to be smoothed
t = 0:Ts:3-Ts # Time vector
R = r.(t) 
Rmat = [R 0*R 0*R]'

Ac = [0.0 1.0 0.0; 0.0 0.0 1.0; 0.0 0.0 0.0]
Bc = [0.0; 0.0; 1.0]

Cc = [1.0 0 0; 0 1 0; 0 0 1]
mpc = LinearMPC.MPC(Ac, Bc, Float64(Ts); C=Cc, Np=50, Nc=50)

set_objective!(mpc; Q=[1e4, 0, 0], R=[1e-6], Rr=[1e-4], Qf=[1e8, 1e-2, 1e-3])
set_input_bounds!(mpc; umin=[-x⃛M], umax=[x⃛M])
add_constraint!(mpc;
                Ax=[0.0 1.0 0.0],
                lb=[-ẋM], ub=[ẋM],
                soft=false)
add_constraint!(mpc;
                Ax=[0.0 0.0 1.0],
                lb=[-ẍM], ub=[ẍM],
                soft=false)

LinearMPC.set_prestabilizing_feedback!(mpc)
#mpc.settings.reference_preview = true

setup!(mpc)

sim1 = LinearMPC.Simulation(mpc; r=Rmat, N=600)
plot(sim1)

LinearMPC.move_block!(mpc,ones(50)) # Should make no difference?
sim2 = LinearMPC.Simulation(mpc; r=Rmat, N=600)
plot!(sim2, c=2)

hline!([ẋM, -ẋM], sp=2, lab="", l=:dash)
hline!([ẍM, -ẍM], sp=3, lab="", l=:dash)
hline!([x⃛M, -x⃛M], sp=4, lab="", l=:dash)

display(norm(sim1.xs-sim2.xs))

[darnstrom]

Just realized I commented out the wrong line... Now I can reproduce the error.

[darnstrom]

Should be fixed in c5cff0c!

[baggepinnen]

I can confirm that it's fixed, but I found another one. Here, I perform three simulations, the first two are the same as above, except that the move blocks are a bunch of ones followed bu a bunch of 2s move_block!(mpc,[ones(100); 2ones(50)]). The 2s in the end of the horizon have negligible impact on the simulation, as expected. In the third simulation, I have added a constraint that the input is zero at the last 4 steps of the 150 long horizon, which again should have negligible impact with such a long horizon

add_constraint!(mpc;
    Au=[1.0;;],  # 
    lb=[0], ub=[0],
    soft=false, ks=Np-4:Np) 

If LinearMPC.set_prestabilizing_feedback!(mpc) is used, this has a large effect on the result, but if I comment it out all three simulations are once again identical as expected.

using LinearMPC

Np = 150
ẍM = 50    # Maximum acceleration
ẋM = 10    # Maximum velocity
x⃛M = 8e3
Ts = 0.005 # Sample time
r(t) = 2.5 + 3 * (t - floor(t)) # Reference to be smoothed
t = 0:Ts:3-Ts # Time vector
R = r.(t) 
Rmat = [R 0*R 0*R]'

Ac = [0.0 1.0 0.0; 0.0 0.0 1.0; 0.0 0.0 0.0]
Bc = [0.0; 0.0; 1.0]

Cc = [1.0 0 0; 0 1 0; 0 0 1]
mpc = LinearMPC.MPC(Ac, Bc, Float64(Ts); C=Cc, Np=Np, Nc=Np)

set_objective!(mpc; Q=[1e4, 0, 0], R=[1e-6], Rr=[1e-4], Qf=[1e8, 1e-2, 1e-3])
set_input_bounds!(mpc; umin=[-x⃛M], umax=[x⃛M])
add_constraint!(mpc;
    Ax=[0.0 1.0 0.0],
    lb=[-ẋM], ub=[ẋM],
    soft=false)
add_constraint!(mpc;
    Ax=[0.0 0.0 1.0],
    lb=[-ẍM], ub=[ẍM],
    soft=false)

LinearMPC.set_prestabilizing_feedback!(mpc)
mpc.settings.reference_preview = true

setup!(mpc)

sim = LinearMPC.Simulation(mpc; r=Rmat, N=600)
plot(sim)

LinearMPC.move_block!(mpc,[ones(100); 2ones(50)]) # Should make no difference
sim = LinearMPC.Simulation(mpc; r=Rmat, N=600)
plot!(sim, c=2)

hline!([ẋM, -ẋM], sp=2, lab="", l=:dash)
hline!([ẍM, -ẍM], sp=3, lab="", l=:dash)
hline!([x⃛M, -x⃛M], sp=4, lab="", l=:dash)



## Enforce zero jerk at end of horizon

add_constraint!(mpc;
    Au=[1.0;;],  # 
    lb=[0], ub=[0],
    soft=false, ks=Np-4:Np)  # Enforce zero jerk at end of horizon
setup!(mpc)
sim = LinearMPC.Simulation(mpc; r=Rmat, N=600)
plot!(sim, c=3)

[darnstrom]

This is probably due to different behaviour beyond the control horizon and prediction horizon when a prestabilizing feedback is used or not.
When you set the move blocks as above, you essentially get a prediction horizon of 150 and a control horizon of 125. The current behaviour of LinearMPC.jl is to let u_k = -Kx+c beyond the control horizon (which is good for regulation, but not very good for reference tracking, as you observe.) If there is no prestabilizing feedback, we simply have that the last u is held constant for the remainder of the prediction horizon.

Maybe it would be nice to also hold u constant for the prestabilized based if one is doing reference tracking. The way I have in incorporated a prestabilizing feedback make this change not as straightforward, but if we think it is a worthwhile feature I could try to add it.

[baggepinnen]

I see, I had thought that the prestabilizing feedback was exclusively for numerical performance and didn't have any semantic meaning, but it actually does then.

but if we think it is a worthwhile feature I could try to add it.

I don't think I strictly need this feature at this point, but detailing this effect in the docs might be worthwhile. BTW, the deployment of the docs appears to be broken (tagbot issues?), the last tagged docs build is 0.7.0 and this is also the last tag on github https://github.com/darnstrom/LinearMPC.jl/releases

[darnstrom]

Ok! We keep this as is then for now. I will add a note about it in the docs (also, I will fix the tagging issue.)
What do you think about also adding a @warning in the code for the cases when Nc < Np, a prestabilizing feedback is set, and mpc.settings.reference_tracking = true? This could also help to flag that some unintended behaviour might happen.

[baggepinnen]

What do you think about also adding a @warning

Sounds very reasonable!

I don't think my case above is equivalent to having a shorter control horizon, the last block is not one large block, it's several small length-2 blocks