Offset-free tracking

codegen/mpc_observer.c

@@ -1,23 +1,23 @@
 void mpc_predict_state(c_float* state, c_float* control, c_float* disturbance){
     int i,j,disp=0;
-    c_float state_old[N_STATE];
-    for(i=0;i<N_STATE;i++) state_old[i] = state[i];
-    for(i=0,disp=0;i<N_STATE;i++){
+    c_float state_old[N_OBSERVER_STATE];
+    for(i=0;i<N_OBSERVER_STATE;i++) state_old[i] = state[i];
+    for(i=0,disp=0;i<N_OBSERVER_STATE;i++){
         state[i] = MPC_PLANT_DYNAMICS[disp++];
-        for(j=0;j<N_STATE;j++) state[i] += MPC_PLANT_DYNAMICS[disp++]*state_old[j];
-        for(j=0;j<N_CONTROL;j++) state[i] += MPC_PLANT_DYNAMICS[disp++]*control[j];
-        for(j=0;j<N_DISTURBANCE;j++) state[i] += MPC_PLANT_DYNAMICS[disp++]*disturbance[j];
+        for(j=0;j<N_OBSERVER_STATE;j++) state[i] += MPC_PLANT_DYNAMICS[disp++]*state_old[j];
+        for(j=0;j<N_OBSERVER_CONTROL;j++) state[i] += MPC_PLANT_DYNAMICS[disp++]*control[j];
+        for(j=0;j<N_OBSERVER_DISTURBANCE;j++) state[i] += MPC_PLANT_DYNAMICS[disp++]*disturbance[j];
     }
 }
 
 void mpc_correct_state(c_float* state, c_float* measurement, c_float* disturbance){
     int i,j,disp_C=0,disp_K=0;
-    c_float innovation, state_old[N_STATE];
-    for(i=0;i<N_STATE;i++) state_old[i] = state[i];
+    c_float innovation, state_old[N_OBSERVER_STATE];
+    for(i=0;i<N_OBSERVER_STATE;i++) state_old[i] = state[i];
     for(j=0;j<N_MEASUREMENT;j++){
         innovation = measurement[j]-MPC_MEASUREMENT_FUNCTION[disp_C++];
-        for(i=0;i<N_STATE;i++) innovation -= MPC_MEASUREMENT_FUNCTION[disp_C++]*state_old[i];
-        for(i=0;i<N_DISTURBANCE;i++) innovation -= MPC_MEASUREMENT_FUNCTION[disp_C++]*disturbance[i];
-        for(i=0;i<N_STATE;i++) state[i] += K_TRANSPOSE_OBSERVER[disp_K++]*innovation;
+        for(i=0;i<N_OBSERVER_STATE;i++) innovation -= MPC_MEASUREMENT_FUNCTION[disp_C++]*state_old[i];
+        for(i=0;i<N_OBSERVER_DISTURBANCE;i++) innovation -= MPC_MEASUREMENT_FUNCTION[disp_C++]*disturbance[i];
+        for(i=0;i<N_OBSERVER_STATE;i++) state[i] += K_TRANSPOSE_OBSERVER[disp_K++]*innovation;
     }
 }

docs/src/manual/observer.md

@@ -6,8 +6,8 @@ Model predictive control requires _all_ the state of the system to be known. All
 ```math
 x_{k+1} = F x_k + G u_k + w_k, \qquad y_k = C x_k + e_k
 ```
-where the process noise $w_k \sim \mathcal{N}(0,Q)$ and the measurement noise $e_k \sim \mathcal{N}(0,R)$. Each time instance, the filter performs two steps: a _correction step_ and a _prediction step_. 
-In the correction steps, the current state estimate $\hat{x}$ is corrected based on a measurement $y$ according to   
+where the process noise $w_k \sim \mathcal{N}(0,Q)$ and the measurement noise $e_k \sim \mathcal{N}(0,R)$. Each time instance, the filter performs two steps: a _correction step_ and a _prediction step_.
+In the correction steps, the current state estimate $\hat{x}$ is corrected based on a measurement $y$ according to
 ```math
 \hat{x} \leftarrow \hat{x} + K(y-C \hat{x}),
 ```
@@ -28,8 +28,42 @@ mpc.set_state_observer(Q=Q, R=R)
 
 where `Q` and `R` are covariance matrices for the process noise and measurement noise, respectively. These are used as tuning variables, where the relative size of the elements in `Q` and `R` determines if the prediction or the measurements should be trusted more. By default, `set_state_observer!` uses $F$, $G$, and $C$ from the MPC structure. It is possible to override this by passing the optional arguments `F`,`G`,`C` to `set_state_observer!`.
 
+## Offset-free tracking
+For offset-free tracking[^Pannocchia15], **LinearMPC.jl** also provides
+
+[^Pannocchia15]: Pannocchia, Gabriele. "Offset-free tracking MPC: A tutorial review and comparison of different formulations." _European control conference (ECC)_ (2015)
+
+Axehill, Daniel, and Hansson, Anders. "A preprocessing algorithm for MIQP solvers with applications to MPC." _43rd IEEE Conference on Decision and Control (CDC)_ (2004)
+
+```julia
+set_offset_free_observer!(mpc; method=:state_disturbance, Q, R)
+```
+
+This augments the controller model with constant disturbance channels and creates an observer that estimates both the nominal state and the disturbance. The estimated disturbance is then fed automatically into `compute_control`, so the closed-loop call sequence stays the same as for a standard state observer:
+
+```julia
+x = correct_state!(mpc, y)
+u = compute_control(mpc, x; r=[0.5])
+predict_state!(mpc, u)
+```
+
+The keyword `method` selects the formulation:
+
+- `:state_disturbance` uses the equivalent disturbance-model realization of the state-disturbance observer
+- `:velocity` uses the equivalent disturbance-model realization of the velocity form with `Ke = I`
+- `:output_disturbance` adds a pure output-bias model when the rank condition is satisfied
+- `:general` accepts user-provided `Bd` and `Cd`
+
+The current disturbance estimate can be inspected with
+
+```julia
+dhat = get_estimated_disturbance(mpc)
+```
+
+The script `example/offset_free_tracking.jl` compares nominal tracking with the offset-free `:velocity` formulation on a disturbed double integrator.
+
 ## Getting, setting, correcting, and predicting state
-The current state of the observer is accessed with 
+The current state of the observer is accessed with
 ```julia
 x = get_state(mpc)
 ```
@@ -44,27 +78,47 @@ Given a measurement `y`, the state of the observer can be corrected with
 correct_state!(mpc,y)
 ```
 
-Given a control action `u`, the next state can be predicted with 
+Given a control action `u`, the next state can be predicted with
 ```julia
 predict_state!(mpc,u)
 ```
 
-## Using the observer  
-Typically, you want to do a correction step before computing a new control action, and then use the new control action to predict the next state. A typical sequence of calls are therefore 
+## Using the observer
+Typically, you want to do a correction step before computing a new control action, and then use the new control action to predict the next state. A typical sequence of calls are therefore
 ```julia
 x = correct_state!(mpc,y)
 u = compute_control(mpc,x)
 predict_state!(mpc,u)
 ```
 
 ## Code generation
-If an observer has been set, the `codegen` function will in addition to C-code for the MPC also generate C-code for the observer. Specifically the C functions `mpc_correct_state(state, measurement, disturbance)` and  `mpc_predict_state(state, control, disturbance)` will be generated, where `state`, `measurement`, `control` and `disturbance` are floating-point arrays. The updated state will be available in the floating-point array `state` after calling the functions. 
+If an observer has been set, the `codegen` function will in addition to C-code for the MPC also generate C-code for the observer. Specifically the C functions `mpc_correct_state(state, measurement, disturbance)` and  `mpc_predict_state(state, control, disturbance)` will be generated, where `state`, `measurement`, `control` and `disturbance` are floating-point arrays. The updated state will be available in the floating-point array `state` after calling the functions.
+
+For standard state observers, `state` has length `N_STATE` and `disturbance` has length `N_DISTURBANCE`, exactly as in the generated MPC API.
 
-Similar to the previous section, the following is a typical sequence of calls when using an observer together with `mpc_compute_control` 
+For offset-free observers, the generated observer state is **augmented**. In that case, the generated C API also provides
+
+```c
+void mpc_get_estimated_state(c_float* state, c_float* observer_state);
+void mpc_get_estimated_disturbance(c_float* disturbance, c_float* observer_state, c_float* measured_disturbance);
+int mpc_compute_control_observer(...);
+```
+
+so that the estimated disturbance can be assembled automatically and control can be computed directly from the augmented observer state.
+
+Similar to the previous section, the following is a typical sequence of calls when using an observer together with `mpc_compute_control`
 
 ```c
 mpc_correct_state(state,measurement,disturbance)
 mpc_compute_control(control,state,reference,disturbance)
 mpc_predict_state(state,control,disturbance)
 ```
 Note for cases when there is no distrubance, one can pass `NULL` instead of `disturbance` as last argument to the functions.
+
+For an offset-free observer the corresponding sequence is
+
+```c
+mpc_correct_state(observer_state, measurement, measured_disturbance);
+mpc_compute_control_observer(control, observer_state, reference, measured_disturbance);
+mpc_predict_state(observer_state, control, measured_disturbance);
+```

src/LinearMPC.jl

@@ -24,6 +24,7 @@ export set_binary_controls!
 export set_disturbance!, set_x0_uncertainty!
 export set_terminal_cost!,set_prestabilizing_feedback!
 export set_state_observer!
+export set_offset_free_observer!
 export set_operating_point!, set_offset!
 export move_block!,set_labels!
 export settings!
@@ -49,6 +50,7 @@ include("robust.jl");
 include("observer.jl");
 export predict_state!,correct_state!
 export set_state!,get_state,update_state!
+export get_estimated_disturbance
 
 using PrecompileTools
 @setup_workload begin

src/codegen.jl

@@ -138,7 +138,7 @@ int mpc_compute_control(c_float* control, c_float* state, c_float* reference, c_
 
     if !isnothing(mpc.state_observer)
         @printf(fh, "#define N_CONTROL %d\n",mpc.model.nu);
-        codegen(mpc.state_observer,fh,fsrc)
+        codegen(mpc.state_observer,mpc,fh,fsrc)
     end
 
     @printf(fh, "#endif // ifndef %s\n", hguard);
@@ -226,7 +226,7 @@ function render_mpc_workspace(mpc;fname="mpc_workspace",dir="",fmode="w", float_
     close(fmpc_src)
 
     if !isnothing(mpc.state_observer)
-        codegen(mpc.state_observer,fh,fsrc)
+        codegen(mpc.state_observer,mpc,fh,fsrc)
     end
 
     @printf(fh, "#endif // ifndef %s\n", hguard);

src/explicit.jl

@@ -54,7 +54,9 @@ function form_parameter(mpc::Union{MPC,ExplicitMPC},x,r,d,uprev,l=nothing)
     # Setup parameter vector θ
     nx,nr,nd,nuprev,nl = get_parameter_dims(mpc)
     r = format_reference(mpc, r)
+    d = get_control_disturbance(mpc, d)
     d = isnothing(d) ? zeros(nd) : d
+    length(d) == nd || throw(ArgumentError("Disturbance vector must have length $nd"))
     uprev = isnothing(uprev) ? mpc.uprev[1:nuprev] : uprev[1:nuprev]
     l_vec = format_linear_cost(mpc, l)
     return [x;r;d;uprev;l_vec]

src/observer.jl

@@ -10,6 +10,45 @@ struct KalmanFilter
     x::Vector{Float64} 
 end
 
+struct OffsetFreeObserver
+    estimator::KalmanFilter
+    C::Matrix{Float64}
+    Dd::Matrix{Float64}
+    h_offset::Vector{Float64}
+    nx::Int
+    nd_measured::Int
+    nd_offsetfree::Int
+    formulation::Symbol
+end
+
+function Base.propertynames(observer::OffsetFreeObserver, private::Bool=false)
+    public_names = (:estimator, :C, :Dd, :h_offset, :nx, :nd_measured,
+                    :nd_offsetfree, :formulation, :x, :d, :K)
+    return private ? (public_names..., fieldnames(KalmanFilter)...) : public_names
+end
+
+function Base.getproperty(observer::OffsetFreeObserver, name::Symbol)
+    if name === :x
+        nx = getfield(observer, :nx)
+        return @view getfield(observer, :estimator).x[1:nx]
+    elseif name === :d
+        nx = getfield(observer, :nx)
+        ndo = getfield(observer, :nd_offsetfree)
+        return @view getfield(observer, :estimator).x[nx+1:nx+ndo]
+    elseif name === :K
+        return getfield(observer, :estimator).K
+    elseif name in fieldnames(OffsetFreeObserver)
+        return getfield(observer, name)
+    elseif name in fieldnames(KalmanFilter)
+        return getproperty(getfield(observer, :estimator), name)
+    else
+        return getfield(observer, name)
+    end
+end
+
+get_estimated_disturbance(::KalmanFilter) = zeros(0)
+get_estimated_disturbance(observer::OffsetFreeObserver) = collect(observer.d)
+
 function KalmanFilter(F,G,C;Gd=nothing,Dd=nothing,f_offset=nothing, h_offset=nothing, x0=nothing, Q=nothing,R=nothing)
     # Solve equation 
     ny,nx = size(C) 
@@ -21,7 +60,7 @@ function KalmanFilter(F,G,C;Gd=nothing,Dd=nothing,f_offset=nothing, h_offset=not
     h_offset = isnothing(h_offset) ? zeros(ny) : h_offset;
     x0 = isnothing(x0) ? zeros(nx) : x0;
     Q = isnothing(Q) ? Matrix{Float64}(I,nx,nx) : matrixify(Q,nx);
-    R = isnothing(R) ? Matrix{Float64}(I,ny,ny) : matrixify(R,nu);
+    R = isnothing(R) ? Matrix{Float64}(I,ny,ny) : matrixify(R,ny);
 
     P,_ = ared(F',C',R,Q);
     K = P*C'/(C*P*C'+R) 
@@ -32,23 +71,64 @@ end
 function set_state!(kf::KalmanFilter,x)
     kf.x .= x
 end
+function set_state!(observer::OffsetFreeObserver, x, d0=nothing)
+    xaug = observer.estimator.x
+    if length(x) == length(xaug)
+        xaug .= x
+    elseif length(x) == observer.nx
+        xaug[1:observer.nx] .= x
+        if isnothing(d0)
+            xaug[observer.nx+1:end] .= 0
+        else
+            length(d0) == observer.nd_offsetfree || throw(ArgumentError("Offset-free disturbance estimate must have length $(observer.nd_offsetfree)"))
+            xaug[observer.nx+1:end] .= d0
+        end
+    else
+        throw(ArgumentError("Observer state must have length $(observer.nx) or $(length(xaug))"))
+    end
+    return observer.x
+end
+
+function get_measured_disturbance(observer::OffsetFreeObserver, d)
+    ndm = observer.nd_measured
+    isnothing(d) && return ndm == 0 ? nothing : zeros(ndm)
+    if length(d) == ndm
+        return d
+    elseif length(d) == ndm + observer.nd_offsetfree
+        return d[1:ndm]
+    else
+        throw(ArgumentError("Disturbance vector must have length $ndm or $(ndm + observer.nd_offsetfree)"))
+    end
+end
+
 function predict!(kf::KalmanFilter,u,d=nothing)
     kf.x .= kf.F*kf.x + kf.G*u +kf.f_offset
     isnothing(d) || (kf.x .+= kf.Gd*d)
     return kf.x
 end
+function predict!(observer::OffsetFreeObserver,u,d=nothing)
+    predict!(observer.estimator,u,get_measured_disturbance(observer,d))
+    return observer.x
+end
 
 function correct!(kf::KalmanFilter,y,d=nothing)
     inov = y - kf.C*kf.x - kf.h_offset
     isnothing(d) || (inov .-= kf.Dd*d)
     kf.x .+= kf.K*inov
 end
+function correct!(observer::OffsetFreeObserver,y,d=nothing)
+    correct!(observer.estimator,y,get_measured_disturbance(observer,d))
+    return observer.x
+end
 
-function codegen(kf::KalmanFilter,fh,fsrc)
+function render_observer_codegen(kf::KalmanFilter,fh,fsrc)
     ny,nx = size(kf.C)
     nu = size(kf.G,2)
     nd = size(kf.Gd,2)
     @printf(fh, "#define N_MEASUREMENT %d\n",ny);
+    @printf(fh, "#define N_OBSERVER_STATE %d\n",nx);
+    @printf(fh, "#define N_OBSERVER_CONTROL %d\n",nu);
+    @printf(fh, "#define N_OBSERVER_DISTURBANCE %d\n",nd);
     @printf(fh, "extern c_float MPC_PLANT_DYNAMICS[%d];\n",nx*(1+nx+nu+nd));
     @printf(fh, "extern c_float MPC_MEASUREMENT_FUNCTION[%d];\n",ny*(1+nx+nd));
     @printf(fh, "extern c_float K_TRANSPOSE_OBSERVER[%d];\n",ny*nx);
@@ -64,6 +144,81 @@ function codegen(kf::KalmanFilter,fh,fsrc)
     close(fmpc_src)
 end
 
+codegen(kf::KalmanFilter,fh,fsrc) = render_observer_codegen(kf,fh,fsrc)
+codegen(kf::KalmanFilter, mpc::Union{MPC,ExplicitMPC}, fh, fsrc) = render_observer_codegen(kf,fh,fsrc)
+
+function codegen(observer::OffsetFreeObserver, mpc::Union{MPC,ExplicitMPC}, fh, fsrc)
+    render_observer_codegen(observer.estimator, fh, fsrc)
+
+    @printf(fh, "#define N_MEASURED_DISTURBANCE %d\n", observer.nd_measured)
+    @printf(fh, "#define N_OFFSET_FREE_DISTURBANCE %d\n", observer.nd_offsetfree)
+    @printf(fh, "void mpc_get_estimated_state(c_float* state, c_float* observer_state);\n")
+    @printf(fh, "void mpc_get_estimated_disturbance(c_float* disturbance, c_float* observer_state, c_float* measured_disturbance);\n")
+    if mpc.nl > 0
+        @printf(fh, "int mpc_compute_control_observer(c_float* control, c_float* observer_state, c_float* reference, c_float* measured_disturbance, c_float* linear_cost);\n")
+    else
+        @printf(fh, "int mpc_compute_control_observer(c_float* control, c_float* observer_state, c_float* reference, c_float* measured_disturbance);\n")
+    end
+
+    write(fsrc, """
+void mpc_get_estimated_state(c_float* state, c_float* observer_state){
+    int i;
+    for(i=0;i<N_STATE;i++) state[i] = observer_state[i];
+}
+
+void mpc_get_estimated_disturbance(c_float* disturbance, c_float* observer_state, c_float* measured_disturbance){
+    int i;
+    for(i=0;i<N_MEASURED_DISTURBANCE;i++) disturbance[i] = measured_disturbance ? measured_disturbance[i] : 0;
+    for(i=0;i<N_OFFSET_FREE_DISTURBANCE;i++) disturbance[N_MEASURED_DISTURBANCE+i] = observer_state[N_STATE+i];
+}
+""")
+
+    if mpc.nl > 0
+        write(fsrc, """
+int mpc_compute_control_observer(c_float* control, c_float* observer_state, c_float* reference, c_float* measured_disturbance, c_float* linear_cost){
+    c_float state[N_STATE];
+    c_float disturbance[N_DISTURBANCE];
+    mpc_get_estimated_state(state, observer_state);
+    mpc_get_estimated_disturbance(disturbance, observer_state, measured_disturbance);
+    return mpc_compute_control(control, state, reference, disturbance, linear_cost);
+}
+""")
+    else
+        write(fsrc, """
+int mpc_compute_control_observer(c_float* control, c_float* observer_state, c_float* reference, c_float* measured_disturbance){
+    c_float state[N_STATE];
+    c_float disturbance[N_DISTURBANCE];
+    mpc_get_estimated_state(state, observer_state);
+    mpc_get_estimated_disturbance(disturbance, observer_state, measured_disturbance);
+    return mpc_compute_control(control, state, reference, disturbance);
+}
+""")
+    end
+end
+
+function codegen(observer::OffsetFreeObserver, fh, fsrc)
+    throw(ArgumentError("Need the MPC to generate code for OffsetFreeObserver"))
+end
+
+function get_control_disturbance(mpc::Union{MPC,ExplicitMPC}, d=nothing)
+    observer = mpc.state_observer
+    !(observer isa OffsetFreeObserver) && return d
+
+    if isnothing(d)
+        d_measured = observer.nd_measured == 0 ? zeros(0) : zeros(observer.nd_measured)
+        return [d_measured; get_estimated_disturbance(observer)]
+    elseif length(d) == observer.nd_measured
+        return [d; get_estimated_disturbance(observer)]
+    elseif length(d) == mpc.model.nd
+        return d
+    else
+        throw(ArgumentError("Disturbance vector must have length $(observer.nd_measured) or $(mpc.model.nd)"))
+    end
+end
+
+simulation_disturbance_dim(mpc::Union{MPC,ExplicitMPC}) = mpc.state_observer isa OffsetFreeObserver ? mpc.state_observer.nd_measured : mpc.model.nd
+get_estimated_disturbance(mpc::Union{MPC,ExplicitMPC}) = isnothing(mpc.state_observer) ? zeros(0) : get_estimated_disturbance(mpc.state_observer)
+
 """
     predict_state!(mpc,u,d=nothing)
 Predict the state at the next time step if the control `u` is applied.