%%%%   Integrator in state-space plus uncertainty%%%%   Add integrator to state-space MPC via disturbance model%%%%   Disturbance on output only%%%%%%%%   Also require estimate of steady-state values of x and u to meet desired%%%%%  set point based on model%%%%%     x = A x + B u    y = Cx + d       %%%%%     let state  d(k) = d(k+1)  be estimate of disturbance%%%%%     z is given as [xhat;dhat]%%%%%  %%%%%     Observor is   z = Ao*z +Bo*u + L*(y - Co*z);%%%%%%%%%%  Estimate steady-state values%%%%%    [A-I  B ] [x] = [0]%%%%%    [C    0 ] [u] = [r-d]      [x_ss;u_ss] = M(r-dhat)%%%%%%%%%%    Control law is   u = -K(xhat-x_ss) + u_ss%%%%%    _____________________________________________________%%%%     OR   u = -Knew z + Pr r%%%%     __________________________________________________%%%%%%%%     K the underlying control law is within Knew%%%%     Q, R are weighting matrices used to find underlying optimal feedback K%%%%     L observer design is arbitrary (could be improved)%%%%%%%%%    [K,L,Ao,Bo,Co,Do,Knew,Pr] = ssmpc_observer(A,B,C,D,Q,R);%%  %% Author: J.A. Rossiter  (email: J.A.Rossiter@shef.ac.uk)function [K,L,Ao,Bo,Co,Do,Knew,Pr] = ssmpc_observer(A,B,C,D,Q,R);na = size(A,1);nb  = size(B,2);nc = size(C,1);%%% Design control  based on simple modelK = dlqr(A,B,Q,R); %%% Add extra states d to observor model - equivalent to output disturbance%%% d(k+1) = d(k)%%%  y(k) = C x + Du + d Ao = [A,zeros(na,nc);zeros(nc,na),eye(nc)];Bo = [B;zeros(nc,nb)]; Co = [C,eye(nc)];Do=D;%%% Design  observor based on augmented model%%% Note arbitratry design - could be replaced by Kalman if more information%%% available on statistics of noiseL = dlqr(Ao',Co',eye(na+nc),eye(nc)*1e-5);L=L';%%%%% Matrix to estimate steady-state values of u and xM = [A-eye(na),B;C,D];N = [zeros(na,nc);eye(nc)];M = M\N;Mx = M(1:na,:);Mu = M(na+1:na+nc,:);Cde = [zeros(nc,na),eye(nc)];%%%% Control law is   u = -Knew z + Prw rKnew = [K,zeros(nc,nc)] + K*Mx*Cde + Mu*Cde;Pr = K*Mx+Mu;