ssmpc_observor.m

%%%%   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: [e-mail])


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 model
K = 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 noise
L = dlqr(Ao',Co',eye(na+nc),eye(nc)*1e-5);
L=L';

%%%%% Matrix to estimate steady-state values of u and x
M = [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 r
Knew = [K,zeros(nc,nc)] + K*Mx*Cde + Mu*Cde;
Pr = K*Mx+Mu;