CloudRunner

... share your algorithms as a web service

Algorithm: Constrained SLQG Controller

Description:
An implementation of the sequence-based LQG controller for LTI plants subject to integral expectation-type linear state and input constraints and controlled over a TCP-like stochastic network. The controller is described in M. Dolgov, J. Fischer, U. D. Hanebeck, "Sequence-based LQG Control with Linear Integral Constraints over Stochastic Networks" (submitted to ... (show more)
Tags: LQG Constraints; NCS;
Usage: Algorithm is public.
Viewed 2029 times, called 0 times
Upload:
Empty star Empty star Empty star Empty star Empty star
0 votes
Maxim Dolgov
03/18/2014 2:16 p.m. (version #1)

Source Code

File:

The file is not a text file or too large to be displayed here. You can still download the file using the link below.
  1 % main simulation script
  2 
  3 % REMARK:
  4 % This simulation script utilizes the function "discretesample" created by 
  5 % Dahua Lin, On Oct 27, 2008. This function is available on Matlab Central.
  6 % Please download this file before running this example.
  7 
  8 % REMARK:
  9 % The implementation of the sequence-based LQG controller requires YALMIP.
 10 % Please download YALMIP before running this example.
 11 
 12 % load simulation network
 13     NetworkDelayDistribution = load('SimulatedNetworks.mat');
 14     NetworkDelayDistribution = ...
 15                          NetworkDelayDistribution.NetworkDelayDistribution;
 16 
 17 %% Simulation properties
 18 simTime = 40;
 19 caDelayProb = NetworkDelayDistribution;
 20 scDelayProb = NetworkDelayDistribution;
 21 caPacketLength = 6;
 22 scMaxDelay = 5;
 23 
 24 x0Mean = [-100;0];
 25 
 26 %% System properties
 27 A = [1, 1; 0 1];
 28 B = [0;1];
 29 C = [1 0];
 30 
 31 Eww = .2^2*eye(size(A,1));
 32 Evv = .1^2*eye(size(C,1));
 33 Exx = .5^2*eye(size(A,1));
 34 
 35 Q = .1*eye(size(A,1));
 36 R = 1*eye(size(B,2));
 37 
 38 uConstr = 5;  % input constraints -5 <= u <=5 
 39 xConstr = 20; % state constraints -20 <= x_2 <= 20
 40 
 41 cX = zeros(size(A,1),4*simTime,simTime+1);  % state constraints are always for one time step more
 42 cU = zeros(size(B,2),4*simTime,simTime);
 43 cC = zeros(1,4*simTime);
 44 for k = 1:simTime
 45   cU(:,k,k) = 1;
 46   cU(:,simTime+k,k) = -1;
 47   cX(2,2*simTime+k,k+1) = 1;
 48   cX(2,3*simTime+k,k+1) = -1;
 49   cC(k) = uConstr;
 50   cC(simTime+k) = uConstr;
 51   cC(2*simTime+k) = xConstr;
 52   cC(3*simTime+k) = xConstr;
 53 end
 54 
 55 %% Simulation
 56 % create random noises
 57 w = mvnrnd(zeros(size(Eww,1),1),sqrt(Eww),simTime)';
 58 v = mvnrnd(zeros(size(Evv,1),1),sqrt(Evv),simTime)';
 59 
 60 % create initial state
 61 x0 = x0Mean + mvnrnd(zeros(size(A,1),1),sqrt(Exx))';
 62 
 63 % create packet delays
 64 caPacketDelays = discretesample(caDelayProb,simTime)-1;
 65 scPacketDelays = discretesample(scDelayProb,simTime)-1;
 66 
 67 constrainedNCS = NCS_ClassConstrainedTcpNcs(A,B,C,Q,R,x0Mean,Exx,Eww,Evv,simTime,caDelayProb,caPacketLength,scMaxDelay,cX,cU,cC);
 68 results = constrainedNCS.Simulate(x0,w,v,caPacketDelays,scPacketDelays);
 69     
 70     
 71 %% Plot figure
 72 figure
 73 
 74 subplot(3,1,1)
 75 hold on
 76 plot(0:1:simTime,results.realTrajectory(1,:),'b','LineWidth',1.5)
 77 ylabel('x_1')
 78 max1 = max(abs(results.realTrajectory(1,:)));
 79 plot(0:1:simTime,zeros(1,simTime+1),'k')
 80 axis([0 simTime -floor(1.1*max1) floor(1.1*max1)])
 81 
 82 subplot(3,1,2)
 83 hold on
 84 plot(0:1:simTime,results.realTrajectory(2,:),'b','LineWidth',1.5)
 85 ylabel('x_2')
 86 max2 = max(max(abs(results.realTrajectory(2,:))),xConstr) + .1*xConstr;
 87 plot(0:1:simTime,zeros(1,simTime+1),'k')
 88 plot(0:1:simTime,xConstr*ones(1,simTime+1),'k:')
 89 plot(0:1:simTime,-xConstr*ones(1,simTime+1),'k:')
 90 axis([0 simTime -floor(1.1*max2) floor(1.1*max2)])
 91 
 92 subplot(3,1,3)
 93 hold on
 94 plot(1:1:simTime,results.appliedU,'b','LineWidth',1.5)
 95 ylabel('u')
 96 maxu = max(max(abs(results.appliedU)),uConstr)+.1*uConstr;
 97 plot(1:1:simTime,zeros(1,simTime),'k')
 98 plot(1:1:simTime,uConstr*ones(1,simTime),'k:')
 99 plot(1:1:simTime,-uConstr*ones(1,simTime),'k:')
100 axis([1 simTime -floor(1.1*maxu) floor(1.1*maxu)])
  1 classdef NCS_ClassLinearSensor < NCS_ClassComponent
  2 %==========================================================================
  3 % NCS_ClassLinearSensor: maps the state x of a plant to a measured output y
  4 % with a linear projection.
  5 %
  6 % NCS_ClassLinearSensor( C , dimX )
  7 % c_dataPacket = DataInDataOut( x , v )
  8 % 
  9 % REMARK: DataInDataOut must be executed once every time step
 10 %
 11 % AUTHOR: Maxim Dolgov, 12.04.2013
 12 % LAST UPDATE: Maxim Dolgov, 12.04.2013
 13 %==========================================================================
 14 properties (SetAccess = private)
 15   %% input properties
 16     % measurement matrix; (matrix of dimension <dimY> x <dimX>)
 17     C = [];
 18     % dimension of the system state; (positive integer)
 19     dimX = -1;
 20   %% derived properties
 21     % dimension of the measurement; (positive integer)
 22     dimY = -1;
 23     % system time; (positive integer)
 24     k = -1;
 25 end % properties
 26 
 27 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 28 methods
 29   %% NCS_ClassLinearSensor
 30   function s = NCS_ClassLinearSensor(C,dimX)
 31   %========================================================================
 32   % NCS_ClassLinearSensor: makes an object of type NCS_ClassLinearSensor
 33   % 
 34   % Synopsis:
 35   % [s] = NCS_ClassLinearSensor(C,dimX)
 36   %
 37   % Input Parameter:
 38   % - C: measurement matrix; (matrix of dimension <dimY> x <dimX>)
 39   % - dimX: dimension of the system state; (positive integer)
 40   %
 41   % Output Parameter:
 42   % - s: handler to the created object
 43   %========================================================================
 44   
 45     % process and store input parameters
 46     s.ProcessInputs(C,dimX);
 47     
 48     % set simulation time
 49     s.k = 1;
 50   end % function NCS_ClassLinearSensor
 51   
 52   %% DataInDataOut
 53   function packet = DataInDataOut(s,x,v)
 54 	%========================================================================
 55   % DataInDataOut: maps the state x of the plant to the measurement y and
 56   % dispatches the measurement in a data packet (ClassDataPacket).
 57   %
 58   % Synopsis:
 59   % dataPacket = DataInDataOut(x,v)
 60   % 
 61   % Input Parameter:
 62   % - x: system state; (column vector of dimension <dimX>)
 63   % - v: measurement noise; (column vector of dimension <dimY>)
 64   %
 65   % Output Parameter:
 66   % dataPacket: time-stamped data packet (NCS_ClassDataPacket) containing
 67   %             the measurement
 68   % 
 69   % REMARK: this function must be executed once every time step
 70   %========================================================================
 71   
 72     packet = NCS_ClassDataPacket(s.C*x+v, s.k);
 73     
 74     s.k = s.k + 1;
 75   end % function DataInDataOut
 76   
 77   %% Reset
 78   function Reset(s)
 79   %========================================================================
 80   % Reset: puts the sensor in the initial condition
 81   %
 82   % Synopsis:
 83   % Reset()
 84   %
 85   % Input Parameter:
 86   % -- none --
 87   % 
 88   % Output Parameter:
 89   % -- none --
 90   %========================================================================
 91   
 92     s.k = 1;
 93   end % function Reset
 94 end % methods public
 95 
 96 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 97 methods (Access = private)
 98   %% ProcessInputs
 99   function ProcessInputs(s,C,dimX)
100   %========================================================================
101   % ProcessInputs: check and store input parameters
102   % 
103   % Synopsis:
104   % ProcessInputs(C,dimX)
105   %
106   % Input Parameter:
107   % - C: measurement matrix; (matrix of dimension <dimY> x <dimX>)
108   % - dimX: dimension of the system state; (positive integer)
109   %
110   % Output Parameter: 
111   % -- none --
112   %========================================================================
113   
114     % dimX
115     if (~isscalar(dimX))
116       error(['** Input parameter <dimX> (system state dimension) ',...
117              'must be a scalar **'])
118     elseif (dimX <= 0 || abs(dimX - floor(dimX)) ~=0)
119       error(['** Input parameter <dimX> (system state dimension) ',...
120              'must be positive integer **'])
121     else
122       s.dimX = dimX;
123     end  % dimX
124     
125     % C
126     if(size(C,2) ~= dimX)
127       error(['** Dimension mismatch between input parameters <A> ',...
128              '(system matrix) and <C> (measurement matrix) **'])
129     else
130       s.C = C;
131       s.dimY = size(s.C,1);
132     end % C
133     
134   end % function ProcessInputs
135 end % methods private
136 end % classdef
  1 classdef NCS_ClassLinearPlant < NCS_ClassComponent
  2 %==========================================================================
  3 % NCS_ClassLinearPlant: implements an LTI system 
  4 %                       x_{k+1} = Ax_{k} + Bu_{k} + w_{k}
  5 %
  6 % NCS_ClassLinearPlant( A , B , x_0 )
  7 %
  8 % REMARK: DataInDataOut must be executed once every time step
  9 %
 10 % AUTHOR: Maxim Dolgov, 12.04.2013
 11 % LAST UPDATE: Maxim Dolgov, 12.04.2013
 12 %==========================================================================
 13 properties (SetAccess = private)
 14   %% input properties
 15     % state matrix; (matrix of dimension <dimX> x <dimX>)
 16     A = [];
 17     % input matrix; (matrix of dimension <dimX> x <dimU>)
 18     B = [];  
 19     % initial system state; (column vector of dimension <dimX>)
 20     x_0 = [];  
 21 	%% derived properties  
 22     % current system state; (column vector of dimension <dimX>)
 23     x = [];  
 24     % control input dimension; (positive integer)
 25     dimU = [];
 26     % system state dimension; (positive integer)
 27     dimX = [];
 28 end
 29 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 30 methods
 31   %% NCS_ClassLinearPlant
 32   function s = NCS_ClassLinearPlant(A,B,x_0)
 33   %========================================================================
 34   % NCS_ClassLinearPlant: creates a new LTI system object
 35   % 
 36   % Synopsis:
 37   % [s] = NCS_ClassLinearPlant(A,B,x_0);
 38   %
 39   % Input Parameter:
 40   % - A: state matrix; (matrix of dimension <dimX> x <dimX>)
 41   % - B: input matrix; (matrix of dimension <dimX> x <dimU>)
 42   % - x_0: initial system state; (column vector of dimension <dimX>)
 43   %
 44   % Output Parameter:
 45   % - s: handle to the created class object
 46   %========================================================================
 47   
 48       % check input parameters
 49       s.ProcessInputs(A,B,x_0);
 50       s.x = s.x_0;
 51   end % function NCS_ClassLinearPlant
 52 
 53   %% DataInDataOut
 54   function xOut = DataInDataOut(s, u, w)
 55   %========================================================================
 56   % DataInDataOut: computes the new system state according to the input
 57   % value u_k and system noise w_k
 58   % 
 59   % Synopsis:
 60   % [xOut] = DataInDataOut(u,w)
 61   %
 62   % Input Parameter:
 63   % - u: control input; (column vector of dimension <dimU>)
 64   % - w: system noise; (column vector of dimension <dimX>)
 65   %
 66   % Output Parameter:
 67   % - xOut: nea system state; (column vector of dimension <dimX>)
 68   %
 69   % REMARK: this function must be executed once every time step
 70   %========================================================================
 71       % check inputs
 72       % control input
 73       if(size(u,2) ~= 1) || (size(u,1) ~= size(s.B,2))
 74           error(['** Dimension mismatch between input parameter <u> ',...
 75                  '(control input) and <B> (input matrix) **'])
 76       elseif(sum(isnan(u)) > 0)
 77         error(['** Input parameter <u> (control inputs) has ',...
 78                'undefined elements **'])
 79       end
 80       % noise input
 81       if (size(w,2) ~= 1) || (size(w,1) ~= size(s.A,1))
 82           error(['** Dimension mismatch between <w> (system noise)',...
 83                  ' and <x> (system state) **'])
 84       elseif(sum(isnan(u)) > 0)
 85         error(['** Input parameter <w> (system noise) has ',...
 86                'undefined elements **'])
 87       end
 88       % compute new state
 89       xOut = s.A * s.x + s.B * u + w;
 90       s.x = xOut;
 91   end % function DataInDataOut
 92 
 93   %% Reset
 94   function Reset(s,varargin)
 95   %========================================================================
 96   % Reset: reset the system
 97   %
 98   % Synopsis:
 99   % Reset();
100   % Reset(x_0);
101   %
102   % Input Parameter:
103   % - x_0: initial system state; (column vector of dimension <dimX>)
104   % 
105   % Output Parameter:
106   % -- none --
107   %========================================================================
108   
109     nVargin = numel(varargin);
110     % Reset()
111     if (nVargin == 0)
112       s.x = s.x_0;
113     % Reset(x_0)
114     elseif (nVargin == 1)
115       x0 = varargin{1};
116       % check x_0
117       if(min(size(x0)) ~= 1)
118         error(['** Input parameter <x_0> (initial system state)',...
119                ' must be a vector **'])
120       elseif(length(x0) ~= size(s.A,1))
121         error(['** Dimension mismatch between input parameters ',...
122                '<x_0> (initial system state) and <A> (system matrix) **'])
123       else
124         s.x_0 = x0;
125       end % x_0
126       
127       s.x = s.x_0;     
128     else
129       error('** Inappropriate number of input parameters provided **')
130     end % varargin processing
131   end % function Reset
132 end % methods public
133 
134 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
135 methods (Access = private)
136   %% ProcessInputs
137   function ProcessInputs(s,A,B,x_0)
138   %========================================================================
139   % ProcessInputs: check input parameters and store them
140   % 
141   % Synopsis:
142   % ProcessInputs(A,B,x_0);
143   %
144   % Input Parameter:
145   % - A: state matrix; (matrix of dimension <dimX> x <dimX>)
146   % - B: input matrix; (matrix of dimension <dimX> x <dimU>)
147   % - x_0: initial system state; (column vector of dimension <dimX>)
148   % 
149   % Output Parameter:
150   % -- none --
151   %========================================================================
152   
153     % A
154     if(size(A,1) ~= size(A,2))
155       size(A)
156       error('** Input parameter <A> (system matrix) must be square **')
157     else
158       s.A = A;
159       s.dimX = size(s.A,1);
160     end % A
161     
162     % B
163     if(size(B,1) ~= size(s.A,1))
164       error(['** Dimension mismatch between input parameters <A> ',...
165              '(system matrix) and <B> (input matrix) **'])
166     else
167       s.B = B;
168       s.dimU = size(s.B,2);
169     end % B
170       
171     % x_0
172     if(min(size(x_0)) ~= 1)
173       error(['** Input parameter <x_0> (initial system state)',...
174              ' must be a vector **'])
175     elseif(length(x_0) ~= size(s.A,1))
176       error(['** Dimension mismatch between input parameters ',...
177              '<x_0> (initial system state) and <A> (system matrix) **'])
178     else
179       s.x_0 = x_0;
180     end % x_0
181        
182   end % function ProcessInputs
183 end % methods private
184 end % classdef
  1 classdef NCS_ClassDelayedKF < handle
  2 %==========================================================================  
  3 % NCS_ClassDelayedKF: This class represents a kalman-filter that can operate
  4 % with time delayed control inputs and/or time delayed measurements. The 
  5 % delayed kalman-filter calculates the linear minimum mean square state 
  6 % estimate of a system based on a model of that system, the expected 
  7 % control input applied to the system and possibly time-delayed 
  8 % measurements of the state.
  9 %
 10 % The algorithm is described in
 11 % Maryam Moayedia, Yung Kuan Foob, Yeng Chai Soha; Filtering for
 12 % networked control systems with single/multiple measurement packets
 13 % subject to multiple-step measurement delays and multiple packet
 14 % dropouts; International Journal of Systems Science; 2011
 15 % and can be found  <a href="matlab:
 16 % web('http://www.tandfonline.com/doi/abs/10.1080/00207720903513335')"
 17 % >here</a>
 18 %
 19 % CONDITION: The function DataInDataOut has to be executed once every time 
 20 % step.
 21 %
 22 % AUTHOR: Jörg Fischer
 23 % LAST UPDATE: Maxim Dolgov, 26.06.2013
 24 %========================================================================== 
 25 properties(SetAccess = private)
 26   % estimated mean value; (vector of dimension <dimX> and type float)
 27   x_est = [];
 28   % initial mean value; (vector of dimension <dimX> and type float)
 29   x_0 = [];
 30   % error covariance matrix;
 31   % (matrix of dimension <dimX> x <dimX> of floats)
 32   Exx = [];
 33   % state matrix of estimatee;
 34   % (matrix of dimension <dimX> x <dimX> of floats)
 35   A = [];
 36   % input matrix of estimatee;
 37   % (matrix of dimension <dimU> x <dimU> of floats) 
 38   B = [];
 39   % output matrix of estimatee;
 40   % (matrix of dimension <dimU> x <dimU> of floats)
 41   C = [];
 42   % dimension of estimatee state; (positive integer)
 43   dimX = 0;
 44   % dimension of output; (positive integer)
 45   dimY = 0;
 46   % dimension of input; (positive integer)
 47   dimU = 0;
 48   % covariance matrix of measurement noise; 
 49   % (matrix of dimension <dimY> x <dimY> of floats)
 50   Evv = [];
 51   % covariance matrix of process noise; 
 52   % (matrix of dimension <dimX> x <dimX> and of floats)
 53   Eww = [];
 54   % augmented system matrix; 
 55   % (matrix of dimension (dimX * (maxMeasDelay + 1)) x (dimX * (maxMeasDelay + 1))
 56   AAug = [];
 57   % augmented process noise input Matrix
 58   % (matrix of dimension (dimX * (maxMeasDelay + 1)) x (dimX * (maxMeasDelay + 1))
 59   BwAug = [];
 60   % augmented state vector
 61   % (vector of dimension s.dimX x (s.maxMeasDelay + 1), 1)
 62   xAug = [];
 63   % augmented measurement noise covariance
 64   % (matrix of dimension s.dimY x (s.maxMeasDelay + 1))
 65   vAugCov = [];
 66   % augmented error covariance
 67   % (matrix of dimension (s.dimX x (s.maxMeasDelay + 1))^2)
 68   errAugCov = [];
 69   % stacked augmented output matrix
 70   % (3-dim array of dim: s.dimY, s.dimX x (s.maxMeasDelay + 1), s.maxMeasDelay + 1)
 71   arrC = [];
 72   % stacked augmented Measurement Noise Selection Matrix
 73   % (3-dim array of dim: s.dimY, s.dimY x (s.maxMeasDelay + 1), s.maxMeasDelay + 1)
 74   arrD = [];
 75   % helping matrix
 76   % matrix of dimension: (s.dimX x (s.maxMeasDelay + 1) , s.dimY)
 77   Fs = [];
 78   % helping matrix
 79   % matrix of dimension: (s.dimX, (s.maxMeasDelay + 1) s.dimX)
 80   gamma = [];
 81   % delay dependend Kalman gain
 82   % (matrix of dimension s.dimX x s.dimY)
 83   Ls = [];
 84   % maximal time delay of mesurements; (positive integer)
 85   maxMeasDelay = -1;
 86   % number of control inputs possibly applied to the system
 87   % (positive integer)
 88   controlPacketLength = -1;
 89   % weighting factors to calculate expected control inpput
 90   % (vector of dimension s.controlPacketLength)
 91   weight = [];
 92   % finit history of control input sequences applied
 93   % (3-dim matrix of dimension: (s.dimU, s.controlPacketLength, s.controlPacketLength)
 94   uhis = [];
 95 
 96   % 
 97   controlDelayProb = [];
 98 
 99   % tolerance interval for rounding error
100   roundingErr = 0.00000001;
101 end % properties
102 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
103 methods
104   %% NCS_ClassDelayedKF
105   function s = NCS_ClassDelayedKF(A, B, C, Eww, Evv, x_0, x0Cov, maxMeasDelay, ...
106                        controlDelayProb, controlPacketLength)
107 	%========================================================================
108   % NCS_ClassDelayedKF: Creates object of a delayed Kalman filter
109   %
110   % Synopsis:
111   % [s] = NCS_ClassDelayedKF(A, B, C, Eww, Evv, x_0, x0Cov, maxMeasDelay, 
112   %                      delayProb)
113   %
114   % Input Paramter:
115   % - A: state matrix of estimatee; (matrix of dimension <dimX> * <dimX>) 
116   % - B: input matrix of estimatee; (matrix of dimension <dimX> * <dimU>)
117   % - C: output matrix of estimatee;(matrix of dimension <dimY> * <dimX>) 
118   % - Eww: covariance matrix of process noise; 
119   %   (matrix of dimension <dimX> * <dimX>)
120   % - Evv: covariance matrix of measurement noise; 
121   %   (matrix of dimension <dimY> * <dimY>) 
122   % - x_0: initial value of expected mean value; 
123   %   (vector of dimension <dimX> and type float) 
124   % - Exx: initial value of error covariance matrix;
125   %   (matrix of dimension <dimX> * <dimX>)
126   % - maxMeasDelay: maximal time delay of mesurements that can be considered 
127   %   by the filter. Measurements that were subject to longer time delays
128   %   are ignored; (positive integer)
129   %   REMARK: The higher maxMeasDelay the higher the required memory because
130   %           the dimension of the augmented matrices are dependend of
131   %           maxMeasDelay
132   % - controlDelayProb: describes the probabilities of 
133   %   control input time delays. Thereby the i-th entry of s.controlDelayProb is 
134   %   the probability that a delay of i time steps occurs. Sum of vector 
135   %   must be equal to 1; If parameter is omitted it is set to 1 and so
136   %   that control input is deterministic; 
137   %   (vector of positive floats)
138   % - controlPacketLength:
139   %
140   % Output Paramter:
141   % - s: created delayed Kalman filter object; (NCS_ClassDelayedKF) 
142   %========================================================================
143     if nargin >= 1
144     % -------- check input parameter and init object variables -----
145     s.ProcessInputs(A, B, C, Eww, Evv, x_0, x0Cov, maxMeasDelay, ...
146                        controlDelayProb, controlPacketLength);
147     
148     s.x_est = s.x_0;
149       
150     s.vAugCov = kron(eye(s.maxMeasDelay+ 1, s.maxMeasDelay+ 1), s.Evv);
151 
152     % init control input sequence history
153     s.uhis = zeros(s.dimU, s.controlPacketLength, s.controlPacketLength);
154 
155     % ---- init augmented system matrices ----
156     % stacked augmented output matrix
157     s.arrC = zeros(s.dimY, s.dimX * (s.maxMeasDelay + 1), s.maxMeasDelay + 1);
158     for counter = 1:1:(s.maxMeasDelay + 1)
159       s.arrC(:, :, counter) = [zeros(s.dimY, s.dimX * (counter - 1)), ...
160         s.C, zeros(s.dimY, s.dimX * (s.maxMeasDelay + 1 - counter))];
161     end
162     % stacked augmented Measurement Noise Selection Matrix
163     s.arrD = zeros(s.dimY, s.dimY * (s.maxMeasDelay + 1), s.maxMeasDelay + 1);
164     for counter = 1:1:(s.maxMeasDelay + 1)
165       s.arrD(:, :, counter) = [zeros(s.dimY, s.dimY * (counter - 1)), ...
166         eye(s.dimY), zeros(s.dimY, s.dimY * (s.maxMeasDelay + 1 - counter))];
167     end
168     % augmented error covariance
169     s.errAugCov = zeros(s.dimX * (s.maxMeasDelay + 1), s.dimX * ...
170       (s.maxMeasDelay + 1));
171     s.errAugCov(:,:) = kron(ones(s.maxMeasDelay + 1, s.maxMeasDelay + 1), s.Exx);
172     % augmented state vector
173     s.xAug = zeros(s.dimX * (s.maxMeasDelay + 1), 1);
174     s.xAug = kron(ones(s.maxMeasDelay + 1, 1), s.x_est);
175     % init helping matrices
176     s.Fs = zeros(s.dimX * (s.maxMeasDelay + 1) , s.dimY);
177     s.gamma = [eye(s.dimX, s.dimX), zeros(s.dimX, s.dimX * s.maxMeasDelay)];    
178     % augmented system Matrix
179     s.AAug = [s.A, zeros(s.dimX, s.dimX * s.maxMeasDelay); kron(eye(...
180       s.maxMeasDelay), eye(s.dimX)), zeros(s.dimX * s.maxMeasDelay, s.dimX)];
181     % augmented process noise input Matrix
182     s.BwAug = [eye(s.dimX); zeros(s.maxMeasDelay * s.dimX, s.dimX)];
183 
184     % ------ init weighting factors for later cyclic calculation of 
185     % weighted averaged input. The filter uses the steady state 
186     % approximation of the occuring delays. Therefore the occuring delays 
187     % are described as a Markov chain with state r_k and the steady state 
188     % solution is the solution of the equilibrium equation 
189     % r_k = trans_matrix * r_k 
190     % with trans_matrix the delay probability transition matrix.
191     % 1) generating the transition matrix
192     trans = s.CalcDelayTrans(s.controlDelayProb);
193     % 2)  deriving weightings as steady state solution of transistion
194     % matrix
195     % steady state solution is eigenvector of eigenvalue 1
196     [eigenVec, eigenVal] = eig(trans');
197     % bounds have to be inserted because of rounding errors
198     [row, ~] = find((eigenVal > (1 - s.roundingErr)) & ...
199       (eigenVal < (1 + s.roundingErr)));
200     if isempty(row)
201       error(['** Transition matrix for time delays has no steady' ...
202         'state solution. Please check delay probabilities! **']);
203     else
204       % weightings are the normalized eigenvector
205       s.weight = eigenVec(:, row) ./ sum(eigenVec(:, row));
206     end
207     end
208   end % function NCS_ClassDelayedKF
209 
210   %% DataInDataOut
211   function [x_est,xCov] = DataInDataOut(s, uInput, nrMeasPackets, varargin)
212   %========================================================================
213   % DataInDataOut: Function executes one time step of the time delayed 
214   % Kalman filter and calculates the estimated state of the considered
215   % system. Inputs of the function are the control input sequence
216   % generated by the controller and (if available) one or more 
217   % data packets containing the information about the measurements.
218   %
219   % Synopsis: 
220   % [x_est] = Execute(s, uInput, nrMeasPackets, measPacketIn)
221   % [x_est] = Execute(s, uInput, 0)
222   %
223   % Input Paramter:
224   % - s: object of delayed Kalman filter to be executed;
225   %   (implicit); (NCS_ClassDelayedKF) 
226   % - uInput: matrix or data packet with control input sequence sent to
227   %   actuator;
228   %   (matrix of dimension <s.dimU> x <s.controlPacketLength>) or 
229   %   (ClassdataPacket with property value is a matrix of dimension 
230   %   <s.dimU> x <s.controlPacketLength>)
231   %   REMARK: This is NOT a vector with control inputs possibly active
232   %   at the current time step. The filter calculates these out of the
233   %   sequences.
234   % - nrMeasPackets: number of measurement packets received by the
235   %   delayed Kalman filter. If the value is zero than the parameter
236   %   measPacketIn is optional and can be omitted; 
237   %   (positive integer)
238   % - measPacketIn: One or more measurements of the state of the system;
239   %   if nrMeasPackets is 0 than this parameter is not considered or
240   %   checked and can be any size.
241   %   (array with dimension <nrMeasPackets> and of type ClassDataPacket)
242   %   or (any format/content if nrMeasPackets = 0)
243   %
244   % Output Paramter:
245   % - x_est: estimated state of the system; 
246   %   (vector of dimension <s.dimX> and type float)
247   %
248   % CONDITION: The function has to be executed once every time step      
249   %========================================================================
250 
251     % ------ 0) check input parameter
252     if nargin > 4
253       error(['** Execute: %d input parameters - ', ...
254           'only 2 or 3 expected.'], nargin - 1)
255     end
256     % nrMeasPackets
257     if ~isscalar(nrMeasPackets)
258       disp(nrMeasPackets)
259       error('** Input parameter nrMeasPackets not a scalar **')
260     elseif(nrMeasPackets < 0) || ...
261         ((nrMeasPackets - floor(nrMeasPackets)) ~= 0)
262       disp(nrMeasPackets)
263       error(['** Input parameter nrMeasPackets is not a positive '...
264         'integer **'])
265     end
266     % measPacketIn
267     if(nrMeasPackets ~= 0) 
268       if(nargin ~= 4)
269         disp(nrMeasPackets)
270         error('** Execute: input parameter <measPacketIn> is missing')
271       else
272         measPacketIn = varargin{1};
273         for i = 1 : nrMeasPackets
274           if ~isa(measPacketIn(i), 'NCS_ClassDataPacket')
275             disp(measPacketIn(i))
276             error(['** Input parameter measPacketIn is not of type <' ... 
277               'NCS_ClassDataPacket> **'])
278           elseif( any(isnan(measPacketIn(i).value)) ) 
279             disp(measPacketIn(i))
280             error(['** At least one entry of value property of the ' ...
281               'data packet <measPacketIn.value> is empty **'])
282           elseif( ~isequal(size(measPacketIn(i).value), [s.dimY, 1]) )
283             disp(measPacketIn(i))
284             error('** Dimension mismatch of measPacketIn.value **')
285           end
286         end
287       end
288     end
289     % uInput
290     if isa(uInput, 'NCS_ClassDataPacket')
291       if any(any(isnan(uInput.value)))
292          error('** Property <value> of data packet has empty entries **')
293       end
294       if( ~isequal(size(uInput.value), [s.dimU, s.controlPacketLength]) )
295          disp(uInput)
296          disp(s.dimU)
297          disp(s.controlPacketLength)
298          error(['** Dimension mismatch of <value> of data packet ' ...
299             '<uInput> **'])
300       end
301       uSeq = uInput.value;
302     elseif( isequal(size(uInput), [s.dimU, s.controlPacketLength]) )
303       uSeq = uInput;
304     else
305       disp(uInput)
306       error(['** Input parameter uSeq is neither a sutiable vector'...
307       ' nor a suitable data packet of type NCS_ClassDataPacket**']);
308     end
309 
310     % ------- 1) Prediction (always performed)
311     s.Predict(uSeq);
312     % ------- 2) Measurement Update
313     if( nrMeasPackets == 0 )
314       %lost measurement --> no measurement update
315     else
316       % Processing all measurements
317       for i = 1 : nrMeasPackets
318         if( measPacketIn(i).packetDelay <= s.maxMeasDelay )
319           % delay smaller than maximal delay --> measurement usable
320           s.MeasUpdate(measPacketIn(i).value, measPacketIn(i).packetDelay);
321         end
322       end
323     end
324     x_est = s.x_est;
325     xCov = s.errAugCov(1:s.dimX,1:s.dimX);
326   end % function DataInDataOut
327   
328   %% Reset
329   function Reset(s)
330     s.vAugCov = kron(eye(s.maxMeasDelay+ 1, s.maxMeasDelay+ 1), s.Evv);
331     
332     s.x_est = s.x_0;
333 
334     % init control input sequence history
335     s.uhis = zeros(s.dimU, s.controlPacketLength, s.controlPacketLength);
336 
337     % ---- init augmented system matrices ----
338     % stacked augmented output matrix
339     s.arrC = zeros(s.dimY, s.dimX * (s.maxMeasDelay + 1), s.maxMeasDelay + 1);
340     for counter = 1:1:(s.maxMeasDelay + 1)
341       s.arrC(:, :, counter) = [zeros(s.dimY, s.dimX * (counter - 1)), ...
342         s.C, zeros(s.dimY, s.dimX * (s.maxMeasDelay + 1 - counter))];
343     end
344     % stacked augmented Measurement Noise Selection Matrix
345     s.arrD = zeros(s.dimY, s.dimY * (s.maxMeasDelay + 1), s.maxMeasDelay + 1);
346     for counter = 1:1:(s.maxMeasDelay + 1)
347       s.arrD(:, :, counter) = [zeros(s.dimY, s.dimY * (counter - 1)), ...
348         eye(s.dimY), zeros(s.dimY, s.dimY * (s.maxMeasDelay + 1 - counter))];
349     end
350     % augmented error covariance
351     s.errAugCov = zeros(s.dimX * (s.maxMeasDelay + 1), s.dimX * ...
352       (s.maxMeasDelay + 1));
353     s.errAugCov(:,:) = kron(ones(s.maxMeasDelay + 1, s.maxMeasDelay + 1), s.Exx);
354     % augmented state vector
355     s.xAug = zeros(s.dimX * (s.maxMeasDelay + 1), 1);
356     s.xAug = kron(ones(s.maxMeasDelay + 1, 1), s.x_est);
357     % init helping matrices
358     s.Fs = zeros(s.dimX * (s.maxMeasDelay + 1) , s.dimY);
359     s.gamma = [eye(s.dimX, s.dimX), zeros(s.dimX, s.dimX * s.maxMeasDelay)];    
360     % augmented system Matrix
361     s.AAug = [s.A, zeros(s.dimX, s.dimX * s.maxMeasDelay); kron(eye(...
362       s.maxMeasDelay), eye(s.dimX)), zeros(s.dimX * s.maxMeasDelay, s.dimX)];
363     % augmented process noise input Matrix
364     s.BwAug = [eye(s.dimX); zeros(s.maxMeasDelay * s.dimX, s.dimX)];
365 
366     % ------ init weighting factors for later cyclic calculation of 
367     % weighted averaged input. The filter uses the steady state 
368     % approximation of the occuring delays. Therefore the occuring delays 
369     % are described as a Markov chain with state r_k and the steady state 
370     % solution is the solution of the equilibrium equation 
371     % r_k = trans_matrix * r_k 
372     % with trans_matrix the delay probability transition matrix.
373     % 1) generating the transition matrix
374     trans = s.CalcDelayTrans(s.controlDelayProb);
375     % 2)  deriving weightings as steady state solution of transistion
376     % matrix
377     % steady state solution is eigenvector of eigenvalue 1
378     [eigenVec, eigenVal] = eig(trans');
379     % bounds have to be inserted because of rounding errors
380     [row, ~] = find((eigenVal > (1 - s.roundingErr)) & ...
381       (eigenVal < (1 + s.roundingErr)));
382     if isempty(row)
383       error(['** Transition matrix for time delays has no steady' ...
384         'state solution. Please check delay probabilities! **']);
385     else
386       % weightings are the normalized eigenvector
387       s.weight = eigenVec(:, row) ./ sum(eigenVec(:, row));
388     end
389   end % function Reset
390 end % methods public
391 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
392 methods(Access = private)
393   %% Predict
394   function Predict(s, uSeq)
395   %========================================================================
396   % Predict: Performs prediction step of the time delayed Kalman filter.
397   %
398   % Synopsis: 
399   % Predict(s, uSeq)
400   %
401   % Input Paramter:
402   % - s: object of time delayed Kalman filter whose prediction step will
403   %   be executed; 
404   %   (implicit); (NCS_ClassDelayedKF) 
405   % - uSeq: matrix of control input sequence sent to actuator;
406   %   (vector of dimension <s.dimU> x <s.controlPacketLength>)
407   %
408   % Output Parameter:
409   % -- none --
410   %========================================================================
411 
412     % 1) calculate expected value of applied control input
413     s.uhis(:, :, 2:end) = s.uhis(:, :, 1 : (end - 1));
414     s.uhis(:, :, 1) = uSeq;
415     uExp = zeros(s.dimU, 1);
416     for counter = 1 : 1 : s.controlPacketLength
417       uExp = uExp + s.uhis(:, counter, counter) .* s.weight(counter);
418     end
419 
420     % 2) calculate state error due to uncertainty about control input
421     uErrCov = zeros(s.dimX * (s.maxMeasDelay + 1));
422     for counter = 1:s.controlPacketLength
423       uErrCov = uErrCov + s.weight(counter) .* s.BwAug * s.B * ...
424         (s.uhis(:, counter, counter) - uExp) * ...
425         (s.uhis(:, counter, counter) - uExp)' * s.B' * s.BwAug';
426     end
427     % 3) use 1 and 2 to perfrom prediction algorithm
428     s.x_est = s.A * s.x_est + s.B * uExp;
429     s.xAug = [s.x_est; s.xAug(1:(end - s.dimX))];
430     s.errAugCov = s.AAug * s.errAugCov * s.AAug' + uErrCov  ...
431       + s.BwAug * s.Eww * s.BwAug';
432   end % function Predict
433 
434   %% MeasUpdate
435   function MeasUpdate(s, yValue, timeDelay)
436   %========================================================================
437   % MeasUpdate: Performs filter step of the time delayed Kalman filter.
438   %
439   % Synopsis: 
440   % MeasUpdate(s, yValue, timeDelay)
441   %
442   % Input Paramter:
443   % - s: object of time delayed Kalman filter whose prediction step will
444   %   be executed; 
445   %   (implicit); (NCS_ClassDelayedKF) 
446   % - yValue: measurement value
447   %   (vector of dimension <s.dimY> of type float)
448   % - timeDealy: time step when measurement was generated
449   %   (positive integer)
450   %
451   % Output Parameter:
452   % -- none --
453   %========================================================================
454 
455     % ----- 1) calculated filtered mean value
456     d = timeDelay + 1;
457     s.Ls = s.gamma * s.errAugCov * s.arrC(:,:,d)' * ...
458       inv( s.arrC(:,:,d) * s.errAugCov * s.arrC(:,:,d)' +...  
459       s.arrD(:,:,d) * s.vAugCov * s.arrD(:,:,d)'); %#ok
460     s.x_est = s.x_est + s.Ls * (yValue - s.arrC(:,:,d) * s.xAug); 
461     s.xAug(1:s.dimX) = s.x_est;
462     % ----- 2) calculated new augmented error covariance matrix
463     s.Fs = [s.Ls; zeros(s.dimX * s.maxMeasDelay, s.dimY)];
464     s.errAugCov = (eye(s.dimX * (s.maxMeasDelay + 1)) - ...
465       s.Fs * s.arrC(:,:,d)) * s.errAugCov * (eye(s.dimX * ...
466       (s.maxMeasDelay + 1)) - s.Fs * s.arrC(:,:,d))' + s.Fs * ...
467       s.arrD(:,:,d) * s.vAugCov * s.arrD(:,:,d)' * s.Fs';    
468   end % function MeasUpdate
469 
470   %% ProcessInputs
471   function ProcessInputs(s, A, B, C, Eww, Evv, x_0, Exx, maxMeasDelay, ...
472                        controlDelayProb, controlPacketLength)
473     % A
474       if (size(A,1) ~= size(A,2))
475         size(A)
476         error('** Input parameter <A> (system matrix) must be square **')
477       else
478         s.A = A;
479         s.dimX = size(s.A,1);
480       end  % A
481 
482     % B
483       if (size(B,1) ~= size(s.A,1))
484         error(['** Dimension mismatch between input parameters <A> ',...
485                '(system matrix) and <B> (input matrix) **'])
486       else
487         s.B = B;
488         s.dimU = size(s.B,2);
489       end  % B
490     
491     % C
492       if (size(C,2) ~= size(A,1))
493         error(['** Dimension mismatch between input parameters ',...
494                '<C> (sensor matrix) and <B> (input matrix) **'])
495       elseif(any(isnan(C)))
496         error('** Input parameter <C> (measurement matrix) contains NaN **')
497       else
498         s.C = C;
499         s.dimY = size(s.C,1);
500       end  % C
501     
502     % x_0
503       if (min(size(x_0)) ~= 1)
504         error(['** Input parameter <x_0> (initial system state)',...
505                ' must be a vector **'])
506       elseif (length(x_0) ~= size(A,1))
507         error(['** Dimension mismatch between input parameters ',...
508                '<x_0> (initial system state) and <A> (system matrix) **'])
509       else
510         s.x_0 = x_0;
511       end  % x_0
512     
513     % Exx
514       if (size(Exx,1) ~= size(Exx,2))
515         error(['** Input parameter <Exx> (initial state covariance)',...
516                ' must be square **'])
517       elseif (size(Exx,1) ~= size(A,1))
518         error(['** Dimension mismatch between input parameters <A> ',...
519                '(system state matrix) and <Exx> (initial state ',...
520                'covariance) **'])
521       elseif (sum(sum(Exx<0)))
522         error(['** Elements of the input parameter <Exx> (initial state',...
523                ' covariance) must be positive **'])
524       elseif (sum(sum(Exx ~= Exx')))
525         error(['** Input parameter <Exx> (initial state covariance) ',...
526                'must be symmetric **'])
527       else
528         s.Exx = Exx;
529       end  % Exx
530     
531     % maxMeasDelay
532       if (~isscalar(maxMeasDelay))
533         error(['** Input parameter <maxMeasDelay> (maximum measurement',...
534              ' delay allowed) must be a scalar **'])
535       elseif ((maxMeasDelay - floor(maxMeasDelay)) ~= 0)
536         error(['** Input parameter <maxMeasDelay> (maximum measurement',...
537              ' delay allowed) must be an integer **'])
538       elseif (maxMeasDelay < 0)
539         error(['** Input parameter <maxMeasDelay> (maximum measurement',...
540              ' delay allowed) must be positive **'])
541       else
542         s.maxMeasDelay = maxMeasDelay;
543       end  % maxMeasDelay
544     
545     % Eww
546       if (size(Eww,1) ~= size(Eww,2))
547         error(['** Input parameter <Eww> (system noise covariance) must',...
548                'be square **'])
549       elseif (size(Eww,1) ~= size(A,1))
550         error(['** Dimension mismatch between input parameters <A> ',...
551                '(system matrix) and <Eww> (system noise covariance) **'])
552       elseif (sum(sum(Eww<0)))
553         error(['** Elements of the input parameter <Eww> (system',...
554                ' noise covariance) must be positive **'])
555       elseif (sum(sum(Eww ~= Eww')))
556         error(['** Input parameter <Eww> (system noise covariance) must',...
557                'be symmetric **'])
558       else
559         s.Eww = Eww;
560       end  % Eww
561     
562     % Evv
563       if (size(Evv,1) ~= size(Evv,2))
564         error(['** Input parameter <Evv> (measurement noise covariance)',...
565                ' must be square **'])
566       elseif (size(Evv,1) ~= size(C,1))
567         error(['** Dimension mismatch between input parameters <C> ',...
568                '(measurement matrix) and <Evv> (measurement noise',...
569                'covariance) **'])
570       elseif (sum(sum(Evv < 0)))
571         error(['** Elements of the input parameter <Evv> (measurement',...
572                ' noise covariance) must be positive **'])
573       elseif (sum(sum(Evv ~= Evv')))
574         error(['** Input parameter <Evv> (measurement noise ',...
575                'covariance) must be symmetric **'])
576       else
577         s.Evv = Evv;
578       end  % Evv
579     
580     % controlPacketLength 
581     if (~isscalar(controlPacketLength))
582       error(['** Input parameter <controlPacketLength> (control sequence ',...
583              'packet length) must be a scalar **'])
584     elseif (controlPacketLength <= 0)
585       error(['** Input parameter <controlPacketLength> (control sequence ',...
586              'packet length) must be positive **'])
587     else
588       s.controlPacketLength = controlPacketLength;
589     end % controlPacketLength
590     
591     % controlDelayProb
592     if (min(size(controlDelayProb)) ~= 1)
593       error(['** Input parameter <controlDelayProb> (packet delay ',...
594              'probability distribution) must be a vector **'])
595     elseif (numel(controlDelayProb) < s.controlPacketLength)
596       error(['** Input parameter <controlDelayProb> (packet delay ',...
597              'probability distribution) is too short **'])
598     elseif (abs(sum(controlDelayProb)-1) > 1e-6)
599       error(['** Sum of elements in the input parameter <controlDelayProb>',...
600              ' (packet delay probability distribution) must be 1 **'])
601     else
602       s.controlDelayProb = controlDelayProb;
603       if (numel(s.controlDelayProb) == s.controlPacketLength)
604         s.controlDelayProb(end+1) = 0;
605       end
606     end % controlDelayProb
607      
608   end % function ProcessInputs
609 end % methods private
610 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
611 methods (Static)
612   %% CalcDelayTrans
613   function [ T ] = CalcDelayTrans( uDelProbs )
614   %========================================================================
615   % CalcDelayTrans: calculates time delay transition matrix
616   % The function determines the transition matrix a the Markov chain whose
617   % state describes the time delay of the combined model of communication
618   % network and actuator.
619   %
620   % Synopsis:
621   % [trans] = CalcDelayTrans( uDelProbs )
622   % 
623   % Input Prameter:
624   % - uDelProbs: describes the probabilities of control input time delays.
625   %   Thereby the i-th entry of delayProb is the probability that a delay 
626   %   of i - 1 time steps occurs. Sum of vector must be equal to 1;
627   %   E.g.: uDelProbls = [0.1 0.9] means that a packet will pass the 
628   %   network without delay with probability 0.1. A delay of one time step
629   %   occurs with probability 0.9.
630   %   (vector of positive floats)
631   %
632   % Output Paramter:
633   % - trans: transition matrix of the Markov chain
634   %   (matrix with dimension numel(uDelProbs) x numel(uDelProbs))
635   %
636   % See also: ClassCommNetwork, NCS_ClassDelayedKF, ClassIMM
637   %========================================================================
638   
639   % check input paramter
640   if( size(uDelProbs, 1) ~= 1 )
641     uDelProbs %#ok<NOPRT>
642     error('** Input parameter uDelProbs is not a row vector **')
643   elseif( min(uDelProbs) < 0 )
644       uDelProbs %#ok<NOPRT>
645       error('** Input parameter uDelProbs has negative entries **')
646   elseif( (round(sum(uDelProbs .* 1000000)) / 1000000) ~= 1 )
647     round(sum(uDelProbs .* 1000000)) / 1000000 %#ok
648     uDelProbs %#ok<NOPRT>
649     error(['** Sum of entries of input paramter uDelProbs is' ...
650       ' not equal to 1 **'])
651   end
652   % determine number of states of the Markov chain
653   numStates = numel(uDelProbs);
654 
655     T = zeros(numStates,numStates);
656 
657     % calculate Transition matrix
658     for i = 1 : numStates
659       for j = 1 : numStates
660         if j >= i +2
661           T(i,j) = 0;
662         elseif j== i + 1
663           T(i,j) = 1 - (sum(uDelProbs(1:i)));
664         elseif j <= i
665           T(i,j) = uDelProbs(j);
666         else
667           error('ERROR');
668         end
669       end
670     end
671   end % function CalcDelayTrans
672 end % methods static
673 end % classdef
 1 classdef NCS_ClassDataPacket
 2 %==========================================================================
 3 % NCS_ClassDataPacket: defines a data packet used to transmit data over a
 4 % communication network described by the class ClassCommNetwork.
 5 %
 6 % [s] = NCS_ClassDataPacket()
 7 % [s] = NCS_ClassDataPacket(value)
 8 % [s] = NCS_ClassDataPacket(value, timeStamp)
 9 %
10 % REMARK: Not implemented as handel class sot that Data packets can be
11 %         copied and changed. Else there can be problems in combination
12 %         with NCS_ClassActuator
13 %
14 %
15 % AUTHOR: Jörg Fischer
16 % LAST UPDATE: Maxim Dolgov, 12.11.2013
17 %==========================================================================
18 
19 properties (Access = public)
20   %% input parameters
21   % time Stamp that indicates time step when data packet was generated;
22   % (positive integer or NaN)
23   timeStamp = -1;
24   % value of data packet. Can be any data; (application dependend data 
25   % type) 
26   value = NaN;
27   % time delay of the data packet, measured in time steps; 
28   % (positive integer or NaN)
29   packetDelay = -1;
30 end % properties
31 
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33   methods
34   %% NCS_ClassDataPacket
35   function s = NCS_ClassDataPacket(varargin)
36   %========================================================================
37   % NCS_ClassDataPacket: creates a handler to an object of type 
38   % (NCS_ClassDataPacket)
39   % 
40   % Synopsis:
41   % [s] = NCS_ClassDataPacket()
42   % [s] = NCS_ClassDataPacket(value)
43   % [s] = NCS_ClassDataPacket(value, timeStamp)
44   %
45   % Input Parameter:
46   % - value: data payload carried by the data packet
47   % - timeStamp: system type indicating when the packet was sent
48   %
49   % Output Parameter:
50   % - s: created data packet
51   %========================================================================
52     if nargin >= 1
53       if(nargin == 1)
54         s.value = varargin{1};
55       elseif nargin == 2
56         s.value = varargin{1};
57         s.timeStamp = varargin{2};
58       else
59         error('** Wrong number of input arguments **')
60       end
61     elseif(nargin == 0)
62       % take default values
63     end
64   end % function NCS_ClassDataPacket
65 end % methods public
66 end % classdef
  1 classdef NCS_ClassConstrainedTcpNcs < handle
  2 %==========================================================================
  3 % NCS_ClassConstrainedTcpNcs: implementation of a complete control loop 
  4 % containing an optimal constrained TCP-controller 
  5 % (NCS_ClassConstrainedTcpController), a controller-actuator network 
  6 % (ClassCommNetwork), an actuator (NCS_ClassActuator) which accepts delayed
  7 % control sequences, a linear plant, a linear sensor, a senor-controller 
  8 % network (ClassCommNetwork) and a state estimator (ClassDelayedKF). 
  9 % This class performs a single simulation run with <simTime> time steps.
 10 %
 11 %
 12 % AUTHOR: Maxim Dolgov
 13 % LAST UPDATE: Maxim Dolgov, 18. Mar. 2014
 14 %==========================================================================
 15 properties (SetAccess = private)
 16   %% input properties
 17     % system matrix; (matrix of dimension <dimX> x <dimX>)
 18     A = [];
 19     % control matrix; (matrix of dimension <dimX> x <dimU>)
 20     B = [];
 21     % sensor matrix; (matrix of dimension <dimY> x <dimX>)
 22     C = [];
 23     % state cost matrix; (matrix of dimension <dimX> x <dimX>)
 24     Q = [];
 25     % control cost matrix; (matrix of dimension <dimU> x <dimU>)
 26     R = [];
 27     % mean value of the initial state; (column vector of dimension <dimX>)
 28     x0Mean = [];
 29     % initial state covariance; (matrix of dimension <dimX> x <dimX>)
 30     x0Cov = [];
 31     % system noise covariance; (matrix of dimension <dimX> x <dimX>)
 32     wCov = []; 
 33     % measurement noise covariance; (matrix of dimension <dimX> x <dimX>)
 34     vCov = [];
 35     % initial system state; (column vector of dimension <dimX>)
 36     x_0 = [];
 37     % simulation time; (positive integer)
 38     simTime = [];
 39     % system noise; (matrix of dimension <dimX> x <simTime>)
 40     w = [];
 41     % measurement noise; (matrix of dimension <dimY> x <simTime>)
 42     v = [];
 43     % delay probability distribution of the controller-actuator network;
 44     % (vector of dimension >= <caPacketLength>)
 45     caDelayProb = [];
 46     % control sequence length; (positive integer)
 47     caPacketLength = [];
 48     % actual delays of the controller-actuator network; 
 49     % (vector of dimension <simTime>)
 50     caPacketDelays = [];
 51     % delay probability distribution of the sensor-controller network;
 52     % (vector)
 53     scDelayProb = [];
 54     % actual delays of the sensor-controller network; 
 55     % (vector of dimension <simTime>)
 56     scPacketDelays = [];
 57     % maximum allowed measurement delay; (positive integer or zero)
 58     scMaxDelay = [];
 59     % system state constraints; 
 60     % (matrix of dimension <dimX> x <numConstraints>)
 61     cX = [];
 62     % constraints for the control inputs;
 63     % (matrix of dimension <dimX> x <numConstraints>)
 64     cU = [];
 65     % borders for each constraint inequality;
 66     % (vector of dimension <numConstraints>)
 67     cC = [];  
 68   %% derived properties and components
 69     % dimension of the system state; (positive integer)
 70     dimX = -1;
 71     % dimension of the control input; (positive integer)
 72     dimU = -1;
 73     % dimension of the measruement; (positive integer)
 74     dimY = -1;
 75     % handle to the controller; (NCS_ClassTcpController)
 76     controller = [];
 77     % handle to the controller-actuator network; (NCS_ClassCommNetwork)
 78     caNetwork = [];
 79     % handle to the controller-actuator data packet; (NCS_ClassDataPacket)
 80     caPacket = [];
 81     % handle to the actuator; (NCS_ClassActuator)
 82     actuator = [];
 83     plant = [];
 84     sensor = [];
 85     scNetwork = [];
 86     scPacket = [];
 87     estimator = [];
 88 end % properties
 89 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 90 methods 
 91   %% NCS_ClassReactiveTcpNcs
 92   function s = NCS_ClassConstrainedTcpNcs(A,B,C,Q,R,x0Mean,x0Cov,wCov,...
 93                vCov,simTime,caDelayProb,caPacketLength,scMaxDelay,cX,cU,cC)
 94   %========================================================================
 95   % NCS_ClassConstrainedTcpNcs: initializes a new object of type 
 96   % <NCS_ClassConstrainedTcpNcs> and returns a handle to it
 97   % 
 98   % Synopsis:
 99   %
100   % Input Parameters:
101   % 
102   % Output Parameters:
103   %========================================================================
104   
105   % The inputs will be checked by the components in this implementation.
106   % Future implementations will check the inputs also here.
107     s.A = A;
108     s.dimX = size(A,1);
109     s.B = B;
110     s.dimU = size(B,2);
111     s.C = C;
112     s.dimY = size(C,1);
113     s.Q = Q;
114     s.R = R;
115     s.x0Cov = x0Cov;
116     s.wCov = wCov;
117     s.vCov = vCov;
118     s.simTime = simTime;
119     s.caDelayProb = caDelayProb;
120     s.caPacketLength = caPacketLength;
121     s.scMaxDelay = scMaxDelay;
122     s.x0Mean = x0Mean;
123     s.cX = cX;
124     s.cU = cU;
125     s.cC = cC;
126   
127 	% initialize components
128     s.controller = NCS_ClassConstrainedTcpController(s.A,s.B,s.Q,s.R,...
129                   s.caDelayProb,s.caPacketLength,s.simTime,s.cX,s.cU,s.cC);
130     s.caNetwork = NCS_ClassCommNetwork(s.simTime,s.caPacketLength+1);
131     s.actuator = NCS_ClassActuator(s.caPacketLength,Inf,zeros(s.dimU,1));
132     s.plant = NCS_ClassLinearPlant(s.A,s.B,zeros(s.dimX,1));
133     s.sensor = NCS_ClassLinearSensor(s.C,s.dimX);
134     s.scNetwork = NCS_ClassCommNetwork(simTime,s.scMaxDelay);
135   end % function NCS_ClassReactiveTcpNcs
136   %% Simulate
137   function results = Simulate(s,x_0,w,v,caPacketDelays,scPacketDelays)
138   %========================================================================
139   % Simulate: performs a simulation run
140   %========================================================================
141   
142   % reset reusable components
143   s.controller.Reset();
144   s.actuator.Reset();
145   s.plant.Reset(x_0);
146   s.sensor.Reset();
147   s.caNetwork.Reset();
148   s.caPacket = NCS_ClassDataPacket();
149   s.scNetwork.Reset();
150   s.scPacket = NCS_ClassDataPacket();
151   % initialize dynamic components
152   s.estimator = NCS_ClassDelayedKF(s.A,s.B,s.C,s.wCov,s.vCov,s.x0Mean,...
153                 s.x0Cov,s.scMaxDelay,s.caDelayProb,s.caPacketLength);
154   
155   % construct output structure
156   results.realTrajectory = zeros(s.dimX,s.simTime+1);
157   results.realTrajectory(:,1) = x_0;
158   results.estimatedTrajectory = zeros(s.dimX,s.simTime+1);
159   results.estimatedTrajectory(:,1) = x_0;
160   results.appliedU = zeros(s.dimU,s.simTime);
161   results.constraints = zeros(1,numel(s.cC));
162   results.nonUdIndices = [];
163   
164   % simulation variables
165   xOut = x_0;
166   mode = s.caPacketLength+1;
167   % main simulation loop
168   for k = 1 : s.simTime
169     s.caPacket = s.controller.DataInDataOut(xOut,mode);
170     [numArrivedCaPackets,arrivedCaPackets] = ...
171                    s.caNetwork.DataInDataOut(s.caPacket,caPacketDelays(k));
172     [uOut,mode] = ...
173             s.actuator.DataInDataOut(numArrivedCaPackets,arrivedCaPackets);
174           
175     if(uOut ~= zeros(numel(uOut),1))
176       results.nonUdIndices = [results.nonUdIndices,k];
177     end
178     xReal = s.plant.DataInDataOut(uOut,w(:,k));
179       
180     s.scPacket = s.sensor.DataInDataOut(xReal,v(:,k));
181     
182     [numArrivedScPackets,arrivedScPackets] = ...
183                    s.scNetwork.DataInDataOut(s.scPacket,scPacketDelays(k));
184                                
185     xOut = s.estimator.DataInDataOut(s.caPacket,numArrivedScPackets,...
186                                      arrivedScPackets);
187 
188     % store simulation results of this step
189     results.realTrajectory(:,k+1) = xReal;
190     results.estimatedTrajectory(:,k+1) = xOut;
191     results.appliedU(:,k) = uOut;
192     for i = 1:numel(s.cC)
193       results.constraints(i) = results.constraints(i)+s.cX(:,i)'*xOut...
194                                +s.cU(:,i)'*uOut;
195     end
196   end % main simulation loop
197   % compute and return costs
198   results.costs = s.ComputeCosts(results.realTrajectory,results.appliedU);
199   end % function Simulate
200 end % methods public
201 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
202 
203 methods (Access = private)
204 
205   %% ComputeCosts
206   function costs = ComputeCosts(s,xTrajectory,uSeq)
207   %========================================================================
208   % ComputeCosts: computes average costs per time step according to an
209   % LQG cost function
210   %
211   % Synopsis:
212   % costs = ComputeCosts(xTrajectory,uSeq);
213   % 
214   % Input Parameters: 
215   % - xTrajectory: state trajectory; 
216   %                (matrix of dimension <dimX> x <simTime+1>)
217   % - uSeq: sequence of applied control inputs;
218   %         (matrix of dimension <dimU> x <simTime>)
219   %
220   % Output Parameters::
221   % - costs: average costs per time step; (positive integer)
222   %========================================================================
223   
224   costs = 0;
225     for i = 1:s.simTime-1
226       costs = costs + ...
227               (xTrajectory(:,i+1))'*s.Q*(xTrajectory(:,i+1)) + ...
228               uSeq(:,i)'*s.R*uSeq(:,i);
229     end
230     costs = costs + ...
231       (xTrajectory(:,s.simTime+1))'*s.Q*(xTrajectory(:,s.simTime+1));
232     costs = costs/s.simTime;
233   end % function ComputeCosts
234 end % methods private
235 end % classdef
  1 classdef NCS_ClassConstrainedTcpController < NCS_ClassComponent
  2 %==========================================================================
  3 % NCS_ClassConstrainedTcpController: implementation of the sequence-based
  4 % LQG controller for plants subject to integral expectation-type linear
  5 % state and input constraints controlled over a TCP-like stochastic
  6 % network. The controller is described in M. Dolgov, J. Fischer, U. D.
  7 % Hanebeck, "Sequence-based LQG Control with Linear Integral Constraints
  8 % over Stochastic Networks" (submitted to CDC 2014)
  9 %
 10 % s = NCS_ClassConstrainedTcpController(A, B, Q, R, delayProb, ...
 11 %                                       packetLength, simTime, a, b, c);
 12 % 
 13 % REMARK: DataInDataOut must be executed once every simulation time step
 14 %         dataPacket = DataInDataOut( x , markovMode )
 15 %
 16 % REMARK: no support for default control input yet
 17 %
 18 % REMARK: this class uses Yalmip optimization toolbox available at
 19 %
 20 % REMARK: provided constraints have the format
 21 %           a(:,constraintNumber,timeStep)
 22 %             size(a) = <dimX> x <numberOfConstraints*simTime> x <simTime+1>
 23 %           b(:,constraintNumber,timeStep)
 24 %             size(b) = <dimU> x <numberOfConstraints*simTime> x <simTime>
 25 %           c(:,numberOfConstraints)
 26 %             size(c) = 1 x <numberOfConstraints>
 27 %         augmented constraints have the format
 28 %           auga(:,constraintNumber,mode,timeStep)
 29 %           augb(:,constraintNumber,mode,timeStep)
 30 %           augc = c
 31 %
 32 % AUTHOR: Maxim Dolgov
 33 % LAST UPDATE: Maxim Dolgov, 18. Mar. 2014
 34 %==========================================================================
 35 properties (SetAccess = protected)
 36   %% input properties
 37     % state matrix; (matrix of dimension <dimX> x <dimX>)
 38     A = [];
 39     % input matrix; (matrix of dimension <dimX> x <dimU>)
 40     B = [];
 41     % system state cost matrix;
 42     % (positive semidefinite matrix of dimension <dimX> x <dimX>)
 43     Q = [];
 44     % input cost matrix;
 45     % (positive definite matrix of dimension <dimU> x <dimU>)
 46     R = [];
 47     % packet delay probability density function of the controller-actuator-
 48     % link; (vector of dimension >= <packetLength>)
 49     delayProb = [];
 50     % control sequence length; (positive integer)
 51     packetLength = -1;
 52   %% derived properties
 53     % simulation time; (positive integer)
 54     simTime = -1;
 55   % constrain related properties
 56     % number of constraints; (positive integer)
 57     numConstraints = -1;
 58     % state constraints; (matrix of dimension <dimX> x <numConstraints>)
 59     a = [];
 60     % input constraints; (matrix of dimension <dimU> x <numConstraunts>)
 61     b = [];
 62     % constrain bounde; (column vector of dimension <numConstraints>)
 63     C = [];
 64     % augmented state constraints; 
 65     % (matrix of dimension <dimState> x <numConstraints> x 
 66     % <packetLength+1>)
 67     auga = [];
 68     % augmented input constraints; 
 69     % (matrix of dimension <dimU*packetLength> x <numConstraints> x 
 70     % <packetLength+1>)
 71     augb = [];
 72     % Lagrange multiplier; (column vector of dimension <numConstraints>)
 73     lambda = [];
 74   % CA-network-actuator system
 75     % mapping for propagation of possible control inputs;
 76     % (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x 
 77     %  <dimU*packetLength*(packetLength-1)/2>)
 78     F = [];
 79     % matrix mapping control sequence to the possible control inputs;
 80     % (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x
 81     %  <dimU*packetLength>)
 82     G = [];
 83   % augmented sate space
 84     % system state of the augmented state space;
 85     % (column vector of dimension <dimState>)
 86     sysState = [];
 87     % system matrix of the augmented state space;
 88     % (matrix of dimension <dimState> x <dimState>)
 89     augA = [];
 90     % control matrix of the augmented state space;
 91     % (matrix of dimension <dimState> x <dimU*packetLength>)
 92     augB = [];
 93     % state cost matrix of the augmented state space;
 94     % (matrix of dimension <dimState> x <dimState>)
 95     augQ = [];
 96     % control sequence cost matrix of the augmented state space;
 97     % (matrix of dimension <dimU*packetLength> x <dimU*packetLength>)
 98     augR = [];
 99 	% Markov chain
100     % transition matrix of the Markov chain;
101     % (matrix of dimension <packetLength+1> x <packetLength+1>)
102     transitionMatrix = [];
103     % current mode of the markov chain; (positive integer)
104     markovMode = -1;
105 	% controller
106     % control covariance; (matrix of dimension )
107     P = [];
108     % constraint matrix;
109     S = [];
110     % constraint covariance;
111     M = [];
112     % Yalmip Lagrange solver
113     ySolver = [];
114     ySolverSedumi = [];
115   % dimensions
116     % dimension of the system state; (positive integer)
117     dimX = -1;
118     % dimension of the control input; (positive integer)
119     dimU = -1;
120     % dimension of the augmented system state; 
121     % (positive integer <dimX+dimU*packetLength*(packetLength-1)/2>)
122     dimState = -1;
123   % system time; (positive integer)
124     k = -1;
125   %% debugging properties
126     debugging = [];
127 end % properties
128 
129 
130 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
131 methods
132   %% NCS_ClassConstrainedTcpController
133   function s = NCS_ClassConstrainedTcpController(A, B, Q, R, delayProb,...
134                packetLength, simTime, a, b, c)
135   %========================================================================
136 	% NCS_ClassConstrainedTcpController: creates and returns a handle to the
137   % object of type (NCS_ClassConstrainedTcpController)
138   %
139   % Synopsis:
140   % [s] = NCS_ClassConstrainedTcpController(A,B,Q,R,delayProb,packetLength)
141   %
142   % Input Parameter:
143   % - A: state matrix; (matrix of dimension <dimX> x <dimX>)
144   % - B: input matrix; (matrix of dimension <dimX> x <dimU>)
145   % - Q: system state cost matrix;
146   %      (positive semidefinite matrix of dimension <dimX> x <dimX>)
147   % - R: input cost matrix;
148   %      (positive definite matrix of dimension <dimU> x <dimU>)
149   % - delayProb: packet delay probability density function of the 
150   %              controller-actuator- link; (vector of dimension )
151   % - packetLength: control sequence length; (positive integer)
152   % - simTime: simulation time; (positive integer)
153   % - a: state constraints; (matrix of dimension <dimX> x <numConstraints>)
154   % - b: input constraints; (matrix of dimension <dimU> x <numConstraints>)
155   % - c: constraint bound; (vector of dimension <numConstraints>)
156   %
157   % Output Parameter:
158   % - s: handle to the object created object; 
159   %      (NCS_ClassConstrainedTcpController)
160   %========================================================================
161     % process and store input parameters
162     s.ProcessInputs(A,B,Q,R,delayProb,packetLength,simTime,a,b,c);
163 
164     % augmented initial system state
165     if(s.packetLength == 1)
166       s.sysState = zeros(s.dimX, 1);
167     else
168       s.sysState = zeros(s.dimX + ...
169         s.dimU*(s.packetLength*(s.packetLength-1)/2),1);
170     end % initial augmented system state
171     s.dimState = length(s.sysState);
172       
173     % augmented state space matrices
174     s.F = s.MakeFMatrix(s.packetLength,s.dimU);
175     s.G = s.MakeGMatrix(s.packetLength,s.dimU);
176     H = s.MakeHMatrices(s.packetLength,s.dimU);
177     J = s.MakeJMatrices(s.packetLength,s.dimU);
178     [s.augA , s.augB] = ...
179       s.MakeAugmentedStateMatrices(s.A,s.B,s.F,s.G,H,J,s.packetLength);
180     [s.augQ , s.augR] = ...
181       s.MakeAugmentedCostMatrices(s.Q,s.R,H,J,s.packetLength);
182     [s.auga, s.augb] = s.AugmentConstraints(H,J);
183       
184     % initialize Markov chain properties
185     s.markovMode = s.packetLength+1;
186     s.transitionMatrix = s.MakeTransitionMatrix(s.delayProb, ...
187       s.packetLength);  
188                                                              
189     % make control related matrices
190     [s.P,s.S,s.M] = s.MakeControlMatrices();
191     
192     % Construct Yalmip Solver
193     for i = 1:s.packetLength+1
194       s.ySolver{i} = s.LagrangeSolver(s.S(:,:,i,1),s.M(:,:,i,1),s.C); 
195     end
196     
197     % set system time
198     s.k = 1;
199   end % function NCS_ClassConstrainedTcpController    
200   
201   
202   %% DataInDataOut
203   function packet = DataInDataOut(s, x, mode)
204   %========================================================================
205 	% DataInDataOut: generates a control input sequence based on the estimate
206   % of the current system state and the Markov mode in the previous time
207   % step. The calculated control sequence is dispatched in a data packet
208   % (NCS_ClassDataPacket).
209   % 
210   % Synopsis:
211   % [packet] = DataInDataOut(x, mode)
212   % 
213   % Input Parameter:
214   % - x: current system state; (column vector of dimension <dimX>)
215   % - mode: previous Markov mode OR its distribution; 
216   %         (positive integer) OR (vector of dim. <packetlength + 1> x 1
217   %
218   % Output Parameter:
219   % - packet: data packet containing the control sequence; 
220   %           (NCS_ClassDataPacket)
221   %
222   % REMARK: this function must be executed once every time step
223   %========================================================================
224     % check inputs
225     % x
226     if(size(x,2) ~= 1)
227       error(['** Input parameter <x> (current system state) is not a ',...
228              'column vector **'])
229     elseif(size(x,1) ~= s.dimX)
230       error(['** Input parameter <x> (current system state) ',...
231              'has wrong dimension **'])
232     elseif(any(isnan(x)))
233       error(['** Input parameter <x> (current system state) contains ',...
234              'NaN **'])
235     end % x
236     
237     % mode
238     if(~(isscalar(mode)))
239       error('** Input parameter <mode> (Markov mode) must be scalar **')
240     elseif(mode < 1 || mode > s.packetLength+1)
241       error(['** Input parameter <mode> (Markov mode) exceeds allowed ',...
242              'range **'])
243     else
244       s.markovMode = mode;
245     end % mode
246     
247     % update augmented system state
248     s.sysState(1:s.dimX) = x;
249      
250     % compute Lagrange multiplier using Yalmip
251     if(s.k == 1)
252       tmpSolver = s.ySolver{s.markovMode};
253       s.lambda = tmpSolver{s.sysState};
254       if(any(isnan(s.lambda)) || any(isinf(s.lambda)))
255         error(['** Control problem is infeasible, some Lagrange ',...
256                'multipliers are NaN **'])
257       end
258     end
259 
260 
261     % generate control sequence
262     P1 = 0;
263     P2 = 0;
264     M1 = 0;
265     for i = 1:s.packetLength+1
266       P1 = P1 + s.transitionMatrix(s.markovMode,i)*(s.augR(:,:,i) + ...
267            s.augB(:,:,i)'*s.P(:,:,i,s.k+1)*s.augB(:,:,i));
268       P2 = P2 + s.transitionMatrix(s.markovMode,i)*(s.augB(:,:,i)'* ...
269            s.P(:,:,i,s.k+1)*s.augA(:,:,i));
270       M1 = M1 + s.transitionMatrix(s.markovMode,i)*(s.augb(:,:,i) + ...
271            s.augB(:,:,i)'*s.M(:,:,i,s.k+1));
272     end
273     controlSequence = -pinv(P1)*(P2*s.sysState + M1*s.lambda);
274     
275     if(any(isnan(controlSequence)) || any(isinf(controlSequence)))
276       error(['** Constrained SLQG: Generated control sequence contains',...
277              ' NaN or Inf **'])
278     end
279       
280     packet = NCS_ClassDataPacket(reshape(controlSequence,s.dimU,...
281              s.packetLength), s.k);
282 
283     % update possible control inputs
284     s.sysState(s.dimX+1:end) = s.F * s.sysState(s.dimX+1:end) + ...
285                                s.G * controlSequence;
286     s.k = s.k + 1;
287   end % function DataInDataOut
288   
289   %% Reset
290   function Reset(s)
291   %========================================================================
292 	% Reset: puts the controller into initial state
293   % 
294   % Synopsis:
295   % Reset()
296   % 
297   % Input Parameter:
298   % -- none --
299   %
300   % Output Parameter:
301   % -- none --
302   %========================================================================
303     s.markovMode = s.packetLength + 1;
304     s.sysState = zeros(s.dimState,1);
305     s.k = 1;
306   end % function Reset
307   
308 end % methods public
309 
310 
311 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
312 methods (Access = protected)  
313   %% ProcessInputs
314   function ProcessInputs(s,A,B,Q,R,delayProb,packetLength,simTime,a,b,c)
315 	%========================================================================
316 	% ProcessInputs: analyzes and stores provided class parameters
317   %
318   % Synopsis:
319   % ProcessInputs(A, B, Q, R, delayProb, packetLength)
320   %
321   % Input Parameter:
322   % - A: state matrix; (matrix of dimension <dimX> x <dimX>)
323   % - B: input matrix; (matrix of dimension <dimX> x <dimU>)
324   % - Q: system state cost matrix;
325   %      (positive semidefinite matrix of dimension <dimX> x <dimX>)
326   % - R: input cost matrix;
327   %      (positive definite matrix of dimension <dimU> x <dimU>)
328   % - delayProb: packet delay probability density function of the 
329   %              controller-actuator- link; (vector of dimension )
330   % - packetLength: control sequence length; (positive integer)
331   % - simTime: simulation time; (positive integer)
332   % -a : state constraints; (matrix of dimension <dimX> x <numConstraints>)
333   % -b : input constraints; (matrix of dimension <dimU> x <numConstraints>)
334   %
335   % Output Parameter:
336   % -- none --
337   %========================================================================
338     % A
339     if(size(A,1) ~= size(A,2))
340       size(A)
341       error('** Input parameter <A> (system matrix) must be square **')
342     elseif(any(isnan(A(:))))
343       error('** Input parameter <A> (system matrix) contains NaN **')
344     else
345       s.A = A;
346       s.dimX = size(s.A,1);
347     end % A
348     
349     % B
350     if(size(B,1) ~= size(s.A,1))
351       error(['** Dimension mismatch between input parameters <A> ',...
352              '(system matrix) and <B> (input matrix) **'])
353     elseif(any(isnan(B(:))))
354       error('** Input parameter <B> (input matrix) contains NaN **')
355     else
356       s.B = B;
357       s.dimU = size(s.B,2);
358     end % B
359     
360     % Check for stabizablity of A by B
361     Co = ctrb(s.A, s.B);
362     unCo = s.dimX - rank(Co);
363     if unCo > 0
364       error('** System (A, B) has uncontrollable eigenvalues **');
365     end
366     
367     % Q
368     if(size(Q,1) ~= size(Q,2))
369       size(Q)        
370       error(['** Input parameter <Q> (system state cost matrix) must',...
371              'be square **'])
372     elseif(size(s.A,1) ~= size(Q,1))
373       error(['** Dimension mismatch between input parameters <A> ',...
374              '(system matrix) and <Q> (system state cost matrix) **'])
375     elseif(sum(sum(Q ~= Q')))
376       error(['** Input parameter <Q> (system state cost matrix) must',...
377              'be symmetric **'])
378     elseif(sum(eig(Q) < 0))
379      error(['** Input parameter <Q> (system state cost matrix) must',...
380             'be positive semidefinite **'])
381     elseif(any(isnan(Q(:))))
382       error(['** Input parameter <Q> (system state cost matrix) ',...
383              'contains NaN **'])
384     else
385       s.Q = Q;
386     end % Q
387     
388     % R
389     if(size(R,1) ~= size(R,2))
390       error(['** Input parameter <R> (input cost matrix) must',...
391              'be square **'])
392     elseif(size(B,2) ~= size(R,1))
393       error(['** Dimension mismatch between input parameters <B> ',...
394              '(input matrix) and <R> (input cost matrix) **'])
395     elseif(sum(sum(R ~= R')))
396       error(['** Input parameter <R> (input cost matrix) must',...
397              'be symmetric **'])
398     elseif(sum(eig(R) <= 0))
399      error(['** Input parameter <R> (input cost matrix) must',...
400             'be positive definite **'])
401     elseif(any(isnan(R(:))))
402       error('** Input parameter <R> (input cost matrix) contains NaN **')
403     else
404       s.R = R;
405     end % R
406     
407     % packetLength
408     if(~isscalar(packetLength))
409       error(['** Input parameter <packetLength> (control sequence ',...
410              'packet length) must be a scalar **'])
411     elseif(packetLength <= 0)
412       error(['** Input parameter <packetLength> (control sequence ',...
413              'packet length) must be positive **'])
414     elseif(any(isnan(packetLength)))
415       error(['** Input parameter <packetLength> (control sequence ',...
416              'packet length) contains NaN **'])
417     else
418       s.packetLength = packetLength;
419     end % packetLength
420       
421     % delayProb
422     if(size(delayProb, 1) ~= 1)
423       error(['** Input parameter <delayProb> (packet delay ',...
424              'probability distribution) must be a row vector **'])
425     elseif(any(isnan(delayProb)))
426       error(['** Input parameter <delayProb> (packet delay ',...
427              'probability distribution) contains NaN **'])
428     elseif(abs(sum(delayProb)-1) > 1e-6)
429       error(['** Sum of elements in the input parameter <delayProb>',...
430              ' (packet delay probability distribution) must be 1 **'])
431     elseif (min(delayProb) < 0)             
432       error('** Input parameter <delayProb> has negative elements **');
433     else
434       if( length(delayProb) <= packetLength )
435         % we have to fill up with zeros
436         probHelp = zeros(1, packetLength + 1);
437         probHelp(1:length(delayProb)) = delayProb(1:end);
438       else
439         % we have to cut the distribution and fill up with an entry so
440         % that vector sums up to one
441         probHelp = delayProb(1:packetLength);
442         probHelp(end + 1) = 1 - sum(delayProb(1:packetLength));
443       end
444       s.delayProb = probHelp;
445     end % delayProb
446     
447     % simTime
448     if(~isscalar(simTime))
449       error(['** Input parameter <simTime> (simulation time)',...
450              ' must be a scalar **'])
451     elseif(simTime <= 0)
452       error(['** Input parameter <simTime> (simulation time)',...
453              ' must be positive **'])
454     elseif(any(isnan(simTime)))
455       error(['** Input parameter <simTime> (simulation time)',...
456              ' contains NaN **'])
457     else
458       s.simTime = simTime;
459     end % simTime
460     
461     % c: constraint bounds;  c(:,numberOfConstraints)
462     if(numel(size(c)) ~= 2)
463       error(['** Input parameter <c> (constraint bounds) must have ',...
464              'two dimensions **']) 
465     elseif(size(c,1) ~= 1 && size(c,2) ~= 1)
466       error(['** Input parameter <c> (constraint bounds) must be a ',...
467              'vector of scalars **']) 
468     elseif(any(isnan(c(:))) || any(isinf(c(:))))
469       error(['** Input parameter <c> (constraint bounds) contains ',...
470              'NaN and Inf **']) 
471     else
472       s.C = c(:);
473       s.numConstraints = numel(c);
474     end % c
475     
476     % a: state constraints;  a(:,constraintNumber,timeStep)
477     if(numel(size(a)) ~= 3)
478       error(['** Input parameter <a> (state constraints) must have ',...
479              'three dimensions **'])
480     elseif(size(a,1) ~= s.dimX || size(a,2) ~= s.numConstraints || size(a,3) ~= s.simTime+1)
481       error(['** Input parameter <a> (state constraints) has wrong ',...
482              'dimension **'])
483     elseif(any(isnan(a(:))) || any(isinf(a(:))))
484       error(['** Input parameter <a> (state constraints) contains ',...
485              'NaN and Inf **'])    
486     else
487       s.a = a;
488     end % a
489     
490     % b: input constraints;  b(:,constraintNumber,timeStep)
491     if(numel(size(b)) ~= 3)
492       error(['** Input parameter <b> (input constraints) must have ',...
493              'three dimensions **'])
494     elseif(size(b,1) ~= s.dimU || size(b,2) ~= s.numConstraints ...
495            || size(b,3) ~= s.simTime)
496       error(['** Input parameter <b> (input constraints) has wrong ',...
497              'dimension **'])
498     elseif(any(isnan(b(:))) || any(isinf(b(:))))
499       error(['** Input parameter <b> (input constraints) contains ',...
500              'NaN and Inf **']) 
501     else
502       s.b = b;
503     end % b
504     
505   end % function ProcessInputs
506 
507   %% MakeControlMatrices
508   function [P,S,M] = MakeControlMatrices(s)
509   %========================================================================
510   % MakeControlMatrices:
511   %========================================================================
512   
513     P = zeros(s.dimState,s.dimState,s.packetLength+1,s.simTime+1);
514     M = zeros(size(s.auga(:,1,1,1),1),s.numConstraints,s.packetLength+1,...
515               s.simTime+1);
516     S = zeros(s.numConstraints,s.numConstraints,s.packetLength+1,...
517               s.simTime+1);
518     
519     % initialize
520     for i = 1:s.packetLength+1
521       P(1:s.dimX,1:s.dimX,i,s.simTime+1) = s.Q;
522       M(:,:,i,s.simTime+1) = s.auga(:,:,i,s.simTime+1);
523     end % i: iteration through modes
524     
525     % compute the matrices
526     for t = 1:s.simTime
527       for i = 1:s.packetLength+1
528         P1 = 0;
529         P2 = 0;
530         P3 = 0;
531         M1 = 0;
532         M2 = 0;
533         S1 = 0;
534         for j = 1:s.packetLength+1
535           P1 = P1 + s.transitionMatrix(i,j)*(s.augQ(:,:,j) + ...
536                 s.augA(:,:,j)'*P(:,:,j,s.simTime-t+2)*s.augA(:,:,j));
537           P2 = P2 + s.transitionMatrix(i,j)*(s.augR(:,:,j) + ...
538                s.augB(:,:,j)'*P(:,:,j,s.simTime-t+2)*s.augB(:,:,j));
539           P3 = P3 + s.transitionMatrix(i,j)*(s.augB(:,:,j)'*...
540                P(:,:,j,s.simTime-t+2)*s.augA(:,:,j));
541           M1 = M1 + s.transitionMatrix(i,j)*(s.augA(:,:,j)'*...
542                M(:,:,j,s.simTime-t+2) + s.auga(:,:,j,s.simTime-t+1));
543           M2 = M2 + s.transitionMatrix(i,j)*(s.augB(:,:,j)'*...
544                M(:,:,j,s.simTime-t+2) + s.augb(:,:,j,s.simTime-t+1));
545           S1 = S1 + s.transitionMatrix(i,j)*S(:,:,j,s.simTime-t+2);
546         end % j: iteration through subsequent mode
547         if(any(isnan(P1(:))) || any(isinf(P1(:))) ||...
548            any(isnan(P2(:))) || any(isinf(P2(:))) ||...
549            any(isnan(P3(:))) || any(isinf(P3(:))) ||...
550            any(isnan(M1(:))) || any(isinf(M1(:))) ||...
551            any(isnan(M2(:))) || any(isinf(M2(:))) ||...
552            any(isnan(S1(:))) || any(isinf(S1(:))))           
553           disp('** something wrong **')
554         end
555         P(:,:,i,s.simTime-t+1) = P1 - P3'*pinv(P2)*P3;
556         M(:,:,i,s.simTime-t+1) = M1 - P3'*pinv(P2)*M2;
557         S(:,:,i,s.simTime-t+1) = S1 - M2'*pinv(P2)*M2;
558       end % i: iteration through current mode
559     end % t: iteration through simulation time
560   end % function MakeControlMatrices
561   
562   %% AugmentConstraints
563   function [auga,augb] = AugmentConstraints(s,H,J)
564   %========================================================================
565   % AugmentConstraints: expresses state and input constraints by means of
566   % the augmented state
567   %
568   % Output Parameter:
569   % - auga: augmented state constraints; (matrix of dimension <dimState> 
570   %         x <numConstraints*simTime> x <packetLength+1> x <simTime+1>)
571   % - augb: augmented input constraints; (matrix of dimension 
572   %         <dimU*packetLength> x <numConstraints*simTime> x 
573   %         <packetLength+> x <simTime>)
574   %========================================================================
575  
576     auga = zeros(s.dimState,s.numConstraints,s.packetLength+1,s.simTime+1);
577     augb = zeros(s.dimU*s.packetLength,s.numConstraints,...
578                  s.packetLength+1,s.simTime);
579     
580     % convert time steps 1..simTime
581     for t = 1:s.simTime
582       for c = 1:s.numConstraints
583         for i = 1:s.packetLength+1
584           auga(:,c,i,t) = [s.a(:,c,t); H(:,:,i)'*s.b(:,c,t)];
585           augb(:,c,i,t) = J(:,:,i)'*s.b(:,c,t);
586         end % i: iteration through modes
587       end
588     end
589     
590     % convert state constraints for time step simTime+1
591     for c = 1:s.numConstraints
592       for i = 1:s.packetLength+1
593         auga(1:s.dimX,c,i,s.simTime+1) = s.a(:,c,s.simTime+1);
594       end % i: iteration through modes
595     end
596   end % function AugmentConstraints
597 end % methods private
598 
599 
600 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
601 methods (Static)
602   
603   function solver = LagrangeSolver(S,M,c)
604   %========================================================================
605   %
606   %========================================================================
607   
608     numConstr = size(S,1);
609     
610     l = sdpvar(numConstr,1);
611     x = sdpvar(size(M,1),1);
612     
613     constr = (l >= 0);
614     cost = .5*l'*S*l + x'*M*l - c'*l;
615     
616     % create optimizer
617     opts = sdpsettings;
618     opts.solver = 'quadprog'; % MATLAB QP solver
619     solver = optimizer(constr,-cost,opts,x,l);
620   end % function LagrangeSolver
621   
622   %% ComputetransitionMatrix
623   function T = MakeTransitionMatrix(delayProb,packetLength)
624   %========================================================================
625 	% MakeTransitionMatrix: computes the transition matrix of the Markov
626 	% chain which governs the packet arrival process at the actuator.
627   % 
628   % Synopsis:
629   % [T] = MakeTransitionMatrix(delayProb, packetLength)
630   % 
631   % Input Parameter:
632   % - delayProb: packet delay probability density function of the 
633   %              controller-actuator- link; (vector of dimension )
634   % - packetLength: control sequence length; (positive integer)
635   %
636   % Output Parameter:
637   % - T: transition matrix of the Markov chain
638   %
639   % T(i,j) = Prob(nextMode = j | currentMode = i)
640   %========================================================================
641     % determine number of states of the Markov chain
642     numStates = packetLength+1;
643     T = zeros(numStates,numStates);
644     % calculate transition matix
645     for i = 1 : numStates
646       for j = 1 : numStates
647         if(j >= i +2)
648           T(i,j) = 0;
649         elseif(j == i + 1)
650           T(i,j) = 1 - (sum(delayProb(1:i)));
651         elseif(j <= i)
652           T(i,j) = delayProb(j);
653         else
654           error('ERROR');
655         end
656       end
657     end
658     T(numStates,numStates) = 1-sum(delayProb(1:numStates-1));
659   end % function MakeTransitionMatrix
660  
661   
662   %% MakeFMatrix
663   function F = MakeFMatrix(packetLength,dimU)
664 	%========================================================================
665 	% MakeFMatrix: makes the matrix F of the system which governs the
666 	% control input selection at the actuator. The matrix F maps performs one
667 	% step iteration of the vector containig possible futrue control inputs.
668   %
669   % Synopsis:
670   % [F] = MakeFMatrix(packetLength,dimU)
671   %
672   % Input Parameter:
673   % - packetLength: control sequence length; (positive integer)
674   % - dimU: dimension of the control value; (positive integer)
675   % 
676   % Output Parameter:
677   % - F: system matrix of the network-actuator subsystem; 
678   %      (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x
679   %       <dimU*packetLength*(packetLength-1)/2>)
680   %========================================================================
681     if(packetLength == 1)
682       F = [];
683     else
684       F = zeros(packetLength-1,1);
685       for i = 1 : packetLength-2
686         F = [F, zeros(size(F,1),packetLength-i);...
687            zeros(packetLength-i-1,size(F,2)), eye(packetLength-i-1),...
688            zeros(packetLength-i-1,1)]; %#ok
689       end   
690     end        
691     F = kron(F,eye(dimU));
692   end % function MakeFMatrix
693  
694   %% MakeGMatrix
695   function G = MakeGMatrix(packetLength,dimU)
696   %========================================================================
697   % MakeGMatrix: matrix G maps the entries of the current control sequence
698   % which are meant to be applied to the plant at future time steps to the
699   % vector containing possible future control inputs.
700   %
701   % Synopsis:
702   % [G] = MakeGMatrix(packetLength,dimU)
703   %
704   % Input Parameter:
705   % - packetLength: control sequence length; (positive integer)
706   % - dimU: dimension of the control value; (positive integer)
707   %
708   % Output Parameter:
709   % - G: input matrix of the network-actuator subsystem;
710   %      (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x 
711   %       <dimU*packetLength>)
712   %========================================================================
713     G = zeros((packetLength*(packetLength-1)/2),packetLength);
714     G(1:(size(G,2)-1) , 2:size(G,2)) = eye(packetLength-1);
715     G = kron(G,eye(dimU));
716   end % function MakeGMatrix
717 
718   
719   %% MakeHMatrices
720   function H = MakeHMatrices(packetLength,dimU)
721   %========================================================================
722   % MakeHMatrices: matrix H selects a control input from the vector of
723   % containing possible control inputs according to the state of the Markov
724   % chain which governs the packet arrival process at the actuator.
725   %
726   % Synopsis:
727   % [H] = MakeHMatrices(packetLength,dimU)
728   %
729   % Input Parameter:
730   % - packetLength: control sequence length; (positive integer)
731   % - dimU: dimension of the control value; (positive integer)
732   %
733   % Output Parameter:
734   % - H: 3D state measurement matrix of the network-actuator subsystem;
735   %      (matrix of dimension <dimU> x 
736   %       <dimU*packetLength*(packetLength-1)/2> x <packetLength+1>) 
737   %
738   % H(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
739   %========================================================================
740     H = zeros(dimU,dimU*(packetLength*(packetLength-1)/2),packetLength+1);
741     
742     for i = 1 : packetLength-1
743       onePos = dimU*(packetLength*(packetLength-1)/2+1-...
744                (packetLength-i+1)*(packetLength-i)/2);
745       H(:,onePos:onePos+dimU-1,i+1) = eye(dimU);
746     end
747   end % function MakeHMatrices
748 
749   
750   %% MakeJMatrices
751   function J = MakeJMatrices(packetLength,dimU)
752   %========================================================================
753   % MakeJMatrices: matrix J selects the first control input from control
754   % input sequence according to the state of the Markov chain which governs
755   % the packet arrival at the actuator.
756   %
757   % Synopsis:
758   % [J] = MakeJMatrices(packetLength,dimU)
759   %
760   % Input Parameter:
761   % - packetLength: control sequence length; (positive integer)
762   % - dimU: dimension of the control value; (positive integer)
763   %
764   % Output Parameter:
765   % - J: 3D input measurement matrix of the network-actuator subsystem;
766   %      (matrix of dimension <dimU> x <dimU*packetLength> x 
767   %       <packetLength+1>) 
768   %
769   % J(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
770   %========================================================================
771     J = zeros(dimU,dimU*packetLength,packetLength+1);
772     J(:,1:dimU,1) = eye(dimU);
773   end % function MakeJMatrices
774   
775   
776   %% MakeAugmentedStateMatrices
777   function [augA,augB] = MakeAugmentedStateMatrices(A,B,F,G,H,J,...
778                                                     packetLength)
779   %========================================================================
780   % MakeAugmentedStateMatrices: calculates the matrices of the augmented
781   % state space which describes the considered NCS as a Markov Jump Linear
782   % System.
783   %
784   % Synopsis:
785   % [augA,augB] = MakeAugmentedStateMatrices(A,B,F,G,H,J,packetLength)
786   %
787   % Input Parameter:
788   % - A: state matrix; (matrix of dimension <dimX> x <dimX>)
789   % - B: input matrix; (matrix of dimension <dimX> x <dimU>)
790   % - F: system matrix of the network-actuator subsystem; 
791   %      (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x
792   %       <dimU*packetLength*(packetLength-1)/2>)
793   % - G: input matrix of the network-actuator subsystem;
794   %      (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x 
795   %       <dimU*packetLength>)
796   % - H: 3D state measurement matrix of the network-actuator subsystem;
797   %      (matrix of dimension <dimU> x 
798   %       <dimU*packetLength*(packetLength-1)/2> x <packetLength+1>) 
799   % - J: 3D input measurement matrix of the network-actuator subsystem;
800   %      (matrix of dimension <dimU> x <dimU*packetLength> x 
801   %       <packetLength+1>) 
802   % - packetLength: control sequence length; (positive integer)
803   %
804   % Output Parameter:
805   % - augA: 3D augmented state space system matrix;
806   %        (matrix of dimension <dimState> x <dimState> x <packetLength+1>)
807   % - augB: 3D augmented state space measurement matrix;
808   %         (matrix of dimension <dimState> x <dimU*packetLength> x
809   %          <packetLength+1>)
810   % 
811   % augA(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
812   % augB(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
813   %========================================================================
814     [numRowsA,numColumnsA] = size([B*H(:,:,1);F]);     
815     dimA = size(A,1);    
816     augA = zeros(numRowsA, dimA+numColumnsA, packetLength+1);
817      
818     [numRowsB,numColumnsB] = size([B*J(:,:,1);G]);      
819     augB = zeros(numRowsB,numColumnsB, packetLength+1);
820     
821     for i = 1:(packetLength+1)
822         augA(1:dimA,1:dimA,i) = A;
823         augA(1:numRowsA,dimA+1:dimA+numColumnsA,i) = [B*H(:,:,i);F];
824        
825         augB(1:numRowsB,1:numColumnsB,i) = [B*J(:,:,i);G];                     
826     end
827   end % function MakeAugmentedStateMatrices
828  
829   
830   %% MakeAugmentedCostMatrices
831   function [augQ,augR] = MakeAugmentedCostMatrices(Q,R,H,J,packetLength)
832   %========================================================================
833   % MakeAugmentedStateMatrices: expresses the cost matrices in terms of the
834   % augmented state space.
835   % 
836   % Synposis:
837   % [augQ,augR] = MakeAugmentedCostMatrices(Q,R,H,J,packetLength)
838   %
839   % Input Parameter:
840   % - Q: system state cost matrix;
841   %      (positive semidefinite matrix of dimension <dimX> x <dimX>)
842   % - R: input cost matrix;
843   %      (positive definite matrix of dimension <dimU> x <dimU>)
844   % - H: 3D state measurement matrix of the network-actuator subsystem;
845   %      (matrix of dimension <dimU> x 
846   %       <dimU*packetLength*(packetLength-1)/2> x <packetLength+1>) 
847   % - J: 3D input measurement matrix of the network-actuator subsystem;
848   %      (matrix of dimension <dimU> x <dimU*packetLength> x 
849   %       <packetLength+1>) 
850   % - packetLength: control sequence length; (positive integer)
851   % 
852   % Output Parameter:
853   % - augQ: 3D augmented state space system state cost matrix;
854   %        (matrix of dimension <dimState> x <dimState> x <packetLength+1>)
855   % - augR: 3D augmented state space control sequence cost matrix;
856   %         (matrix of dimension <dimU*packetLength> x 
857   %          <dimU*packetLength> x <packetLength+1>)
858   %
859   % augQ(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
860   % augR(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
861   %========================================================================
862     sizeQ = size(Q,1) + size(H(:,:,1)'*R*H(:,:,1),1);    
863     
864     augQ = zeros(sizeQ, sizeQ, packetLength+1);
865     
866     augR = zeros(size(J(:,:,1)'*R*J(:,:,1),1),...    
867                size(J(:,:,1)'*R*J(:,:,1),2),...
868                packetLength+1);
869              
870     for i = 1:(packetLength+1)
871         augQ(1:sizeQ,1:sizeQ,i) = ...
872                 [Q,...
873                  zeros(size(Q,1),size(H(:,:,i)'*R*H(:,:,i),2));...
874                  zeros(size(H(:,:,i)'*R*H(:,:,i),1),size(Q,2)),...
875                  H(:,:,i)'*R*H(:,:,i)];
876         augR(:,:,i) = J(:,:,i)'*R*J(:,:,i);
877     end
878   end % function MakeAugmentedCostMatrices
879 end % methods static
880 end % classdef
 1 classdef NCS_ClassComponent < handle
 2 %==========================================================================
 3 % NCS_ClassComponent: This class is the bases for all NCS_Class components.
 4 % Here, all variables and methods can be defined that every component must
 5 % have.
 6 %
 7 % AUTHOR: Jörg Fischer
 8 % LAST UPDATE: Jörg Fischer, 21.11.2013
 9 %==========================================================================
10 properties
11   % On / Off button (scalar of value 1 (On) or 0 (Off)
12   IsOn = 1;
13 end
14 
15   
16 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
17 methods
18   %% NCS_ClassComponent
19   function s = NCS_ClassComponent(onOff)
20   %========================================================================
21   % NCS_ClassComponent: Constructor for a component object
22   %
23   % Synopsis:
24   % [s] = NCS_ClassComponent()
25   % [s] = NCS_ClassComponent(on)
26   % 
27   % Input Parameter:
28   % - onOff: turns component on or off
29   %          (scalar with value 1 (On) or 0 (Off)
30   %
31   % Output Parameter:
32   % - s: object handle for NCS_ClassComponent
33   %========================================================================
34     % enabling zero argument contructor call
35     if nargin > 0
36         s.IsOn = onOff;
37     end
38   end
39 
40   
41   %% set.IsOn
42   %========================================================================
43   % set.IsOn: Set method for property IsOn
44   %
45   % Synopsis:  -- Only implicit called when property is set --
46   % IsOn = value
47   % 
48   % Input Parameter:
49   % - value: turns component on or off
50   %          (scalar with value 1 (On) or 0 (Off)
51   %========================================================================
52   function set.IsOn(s, value)
53     if( ~(value == 0) || ~(value == 1) )
54       error('** Input parameter value must be 1 ("ON") or 0 ("OFF") **');
55     else
56       s.IsOn = value;
57     end
58   end % set.IsOn
59 end % methods
60 end % classdef
  1 classdef NCS_ClassCommNetwork < NCS_ClassComponent
  2 %==========================================================================
  3 % NCS_ClassCommNetwork: This class simulates a communication network to
  4 % send and receive data packets of type NCS_ClassDataPacket. A data packet
  5 % that  enters the network will be subject to a stochastic time delays. If
  6 % the  delay is not longer than the maximal allowed delay the packet is 
  7 % relaesed after the delay, else the packet is lost. The time delay
  8 % probabilities and the maximal time delay is configurable. The output 
  9 % of the network depends on the time delays and can be no packet, one 
 10 % packet or more than one packet. 
 11 %
 12 % REMARK: the function DataInDataOut has to be executed once every 
 13 %         time step.
 14 %
 15 % AUTHOR: Jörg Fischer
 16 % LAST UPDATE: Maxim Dolgov, 30.04.2013
 17 %==========================================================================
 18   
 19 properties (SetAccess = private)
 20   %% input parameters
 21     % length of simulation time in time steps; (positive integer)
 22     simTime = -1;
 23     % maximal time delay;
 24     % The network does not output a data packet that has a longer time 
 25     % delay; (positive integer)
 26     maxDelay = -1;
 27   %% derived parameters
 28     % cell array with data packets to store input data;
 29     % (array of NCS_ClassDataPacket)
 30     nwData = {};
 31     % number of packets that leave the network to current time step;
 32     % Index is the time step; (array of positive integer)
 33     numPacketsOut = [];
 34     % current time step; (positive integer)
 35     k = -1;
 36     % debug variable
 37     debugInfo = {};
 38     % used protocol (has no relevance to behaviour of the network. Only
 39     % needed for other components as actuator and controller to know if age
 40     % of buffer (mode) is available
 41     % (scalar of 1 (TCP) or 0 (UDP)
 42     usesTcp = 1;
 43 
 44     % tolerance interval for rounding error
 45     roundingErr = 0.00000001;
 46 end % properties
 47 
 48 
 49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
 50 methods
 51   %% NCS_ClassCommNetwork
 52   function s = NCS_ClassCommNetwork(simTime, maxDelay, varargin)
 53   %========================================================================
 54   % NCS_ClassCommNetwork: Constructs object of a communication network
 55   %
 56   % Synopsis:
 57   % [s] = NCS_ClassCommNetwork(simTime, maxDelay)
 58   % [s] = NCS_ClassCommNetwork(simTime, maxDelay, protocol)
 59   %
 60   % Input Paramter:
 61   % - Param: structure containing class parameters
 62   % - simTime: time steps of simulation; (positive integer)
 63   % - maxDelay: maximal time delay possible. The value must be equal to
 64   %             or smaller than the number of entries in the vector
 65   %             <delayProb>, if it is defined, or to the maximum delay
 66   %             in the vector packetDelays, if this one is defined;
 67   %             (positive integer)
 68   % - protocol: specifies the used protocol
 69   %             (string with values 'TCP' or 'UDP')
 70   % 
 71   % Output Paramter:
 72   % - s: handler to the created object; (NCS_ClassCommNetwork)
 73   %
 74   %========================================================================
 75     % ------- 0) check input parameters and init data:
 76     if(nargin == 2)
 77       s.ProcessInputs(simTime, maxDelay, 'TCP');
 78     end
 79     if(nargin == 3)
 80       s.ProcessInputs(simTime, maxDelay, varargin{1});
 81     end
 82     if(nargin <= 2)
 83       % init network data
 84       s.numPacketsOut = zeros(simTime, 1);
 85       s.nwData = {};
 86       % To keep track of packets that will leave the network to the same
 87       % time step the network data array is two-dimensional
 88       s.nwData{simTime, 1} = NCS_ClassDataPacket();
 89       % set system time
 90       s.k = 1;
 91     end
 92   end % function NCS_ClassCommNetwork
 93   
 94   %% DataInDataOut
 95   function [numPackets, packetsOut] = DataInDataOut(s, packet, packetDelay)
 96   %========================================================================
 97   % DataInDataOut: Sending and receiving data over the network.
 98   % connection. Every function call 1) a new data packet can be sent
 99   % over the network (optional) and 2) a data packet leaves the network.
100   %
101   % Synopsis: 
102   % [numPackets, packetsOut] = DataInDataOut(packet, packetDelay)
103   % [numPackets, packetsOut] = DataInDataOut([], [])
104   %
105   % Input Paramter:
106   % - packet: data packet sent into the network; (NCS_ClassDataPacket) 
107   % - packetDelay: time delay which will be experienced by the packet;
108   %                (positive integer)
109   % - []: empty parameter, if no packet shall be sent
110   %
111   % Output Paramter:
112   % - numPackets: number of packets that leave the network; (integer) 
113   % - packetsOut: datapacket/s that leave the network;
114   %   (array of NCS_ClassDataPacket)
115   %
116   % REMARK: this function must to be executed once every time step
117   %========================================================================
118     % check input parameters
119     if(isempty(packet))
120       packetReceived = false;
121     else
122       % check packet
123       if(~isa(packet, 'NCS_ClassDataPacket'))
124         error(['** Input parameter <packet> (sent data packet) is not ',...
125                'of class NCS_ClassDataPacket **'])
126       end % check packet
127       
128       % check packetDelay
129       if(~isscalar(packetDelay))
130         error(['** Input parameter <packetDelay> (time delay to ',...
131                ' experience by the packet) must be a scalar **'])
132       elseif(packetDelay < 0)
133         error(['** Input parameter <packetDelay> (time delay to ',...
134                ' experience by the packet) must be positive or zero **'])
135       end % check packetDelay
136       
137       packetReceived = true;
138       
139     end % check input parameters
140     
141     % evaluate wether to put packet in queue
142     if(packetDelay > s.maxDelay) ...
143           || (packetReceived == false)  ...
144           || ((s.k + packetDelay) > s.simTime)
145       % packet lost or no new packet available or packet would
146       % leave network after end of simulation
147       % -> do not take packet in data array
148       % debug Info
149       s.debugInfo{1, s.k} = s.k;
150       s.debugInfo{2, s.k} = NaN;
151       s.debugInfo{3, s.k} = packetDelay;
152     else
153       % warn if packet time stamp does not correspond to transmission time
154       if(packet.timeStamp ~= s.k)
155         error(['** packet time stamp does not correspond to ',...
156               'transmission time **'])
157       end
158       
159       % new packet sent into network:
160       % a) update packet info
161       packet.packetDelay = packetDelay;
162       % b) increment packet output counter for the time step when this 
163       % packet will leave
164       i = s.numPacketsOut(s.k + packetDelay) + 1;
165       s.numPacketsOut(s.k + packetDelay) = i;
166       % c) write data in network data cell array
167       s.nwData{s.k + packetDelay, i} = packet;
168       % Storing data for debug
169       s.debugInfo{1, s.k} = s.k;
170       s.debugInfo{2, s.k} = packet.value;
171       s.debugInfo{3, s.k} = packetDelay;
172     end % put the received packet in queue
173     
174     % network output
175     % first output parameter is number of packets that will leave the
176     % network
177     numPackets = s.numPacketsOut(s.k);
178     % the second output parameter is(are) the packet(s) that will leave
179     % the network to current time step
180     if( numPackets == 0 )
181       % no packet
182       packetsOut = NCS_ClassDataPacket();
183       % storing data for debug
184       s.debugInfo{5, s.k} = packetsOut.value;
185       s.debugInfo{6, s.k} = packetsOut.timeStamp;
186       s.debugInfo{7 , s.k} = packetsOut.packetDelay;
187     else
188       % allocate memory for output packets
189       packetsOut(numPackets) = NCS_ClassDataPacket();
190       for i = 1:numPackets
191         packetsOut(i) = s.nwData{s.k, i};
192         % storing data for debug
193         s.debugInfo{2 + i * 3, s.k} = packetsOut(i).value;
194         s.debugInfo{3 + i * 3, s.k} = packetsOut(i).timeStamp;
195         s.debugInfo{4 + i * 3, s.k} = packetsOut(i).packetDelay;
196       end
197     end
198     s.debugInfo{4, s.k} = '------------';
199 
200     % increment time step
201     s.k = s.k + 1;
202   end % function DataInDataOut
203   
204   %% Reset
205   function Reset(s)
206   %========================================================================
207   % Reset: puts the sensor in the initial condition
208   %
209   % Synopsis:
210   % Reset()
211   %
212   % Input Parameter:
213   % -- none --
214   % 
215   % Output Parameter:
216   % -- none --
217   %========================================================================
218     % init network data
219     s.numPacketsOut = zeros(s.simTime, 1);
220     s.nwData = {};
221     % To keep track of packets that will leave the network to the same
222     % time step the network data array is two-dimensional
223     s.nwData{s.simTime, 1} = NCS_ClassDataPacket();
224     s.k = 1;
225   end % function Reset
226 end % methods public
227 
228 
229 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
230 methods (Access = private)
231   %% ProcessInputs
232   function ProcessInputs(s, simTime, maxDelay, protocol)
233 	%========================================================================
234   % ProcessInputs: analyzes and stores provided class parameters
235   %
236   % Synopsis:  
237   %   ProcessInputs(simTime, maxDelay, protocol)
238   % Input Parameters:
239   %   see Constructor
240   %========================================================================
241     % simTime
242     if (~isscalar(simTime))
243       error(['** Input parameter <simTime> (simulation time) is ',...
244              'not a scalar**'])
245     elseif (simTime < 0)
246       error(['** Input parameter <simTime> (simulation time) must',...
247              ' be positive **'])
248     elseif ((simTime - floor(simTime)) ~= 0)
249       error(['** Input parameter <simTime> (simulation time) must be',...
250              ' an integer **'])
251     else 
252       s.simTime = simTime;
253     end
254     % check simTime
255 
256     % maxDelay
257     if ~isscalar(maxDelay)
258       error(['** Input parameter <maxDelay> (maximum possible packet',...
259              ' delay) is not a scalar **'])
260     elseif((maxDelay < 0) || ...
261            ((maxDelay - floor(maxDelay)) ~= 0))
262       error(['** Input parameter <maxDelay> (maximum possible packet',...
263              ' delay) is not a positive integer **'])
264     else
265       s.maxDelay = maxDelay;
266     end
267     % check maxDelay
268 
269     % protocol
270     if ~ischar(protocol)
271       error(['Input paramter <protocol> must be string with value', ...
272             ' "TCP" or "UDP"'])
273     elseif(strcmpi(protocol, 'UDP'))
274       s.usesTcp = 0;
275     elseif(strcmpi(protocol, 'TCP'))
276       s.usesTcp = 1;
277     else
278       error(['Input paramter <protocol> must be string with value', ...
279             ' "TCP" or "UDP"'])
280     end
281   end % function ProcessInputs 
282 end % methods private
283 
284 
285 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
286 methods (Static)
287   %% DiscreteSample
288   function x = DiscreteSample(p, n)
289   %========================================================================
290   % DiscreteSample: Samples from a discrete distribution: independently 
291   % draws n samples (with replacement) from the distribution specified by 
292   % p, where p is a probability array whose elements sum to 1.
293   %
294   % Synopsis:
295   % [x] = DiscreteSample(p, n)
296   % 
297   % Input Parameter:
298   % - p: probability density function; (vector of dimension K)
299   % - n: number of drawn samples; (positive integer)
300   %
301   % Output Parameter:
302   % - x: samples from a discrete distribution
303   %
304   % REMARK: Created by Dahua Lin, On Oct 27, 2008
305   %========================================================================
306     % parse and verify input arguments
307     assert(isfloat(p), 'discretesample:invalidarg', ...
308         'p should be an array with floating-point value type.');
309 
310     assert(isnumeric(n) && isscalar(n) && n >= 0 && n == fix(n), ...
311         'discretesample:invalidarg', ...
312         'n should be a nonnegative integer scalar.');
313 
314     % main
315     % process p if necessary
316     K = numel(p);
317     if ~isequal(size(p), [1, K])
318         p = reshape(p, [1, K]);
319     end
320 
321     % construct the bins
322     edges = [0, cumsum(p)];
323     s = edges(end);
324     if abs(s - 1) > eps
325         edges = edges * (1 / s);
326     end
327 
328     % draw bins
329     rv = rand(1, n);
330     c = histc(rv, edges);
331     ce = c(end);
332     c = c(1:end-1);
333     c(end) = c(end) + ce;
334 
335     % extract samples
336     xv = find(c);
337 
338     if numel(xv) == n  % each value is sampled at most once
339         x = xv;
340     else                % some values are sampled more than once
341         xc = c(xv);
342         d = zeros(1, n);
343         dv = [xv(1), diff(xv)];
344         dp = [1, 1 + cumsum(xc(1:end-1))];
345         d(dp) = dv;
346         x = cumsum(d);
347     end
348     % randomly permute the sample's order
349     x = x(randperm(n));
350   end % function DiscreteSample
351 end % methods static
352 end % classdef
  1 classdef NCS_ClassActuator < handle
  2 %==========================================================================
  3 % NCS_ClassActuator: This class simulates an actuator in a packet-based 
  4 % NCS configuration. The input of the actuator are data packets that
  5 % consist of time-stamped sequences of control inputs. Output is a 
  6 % single control input that is applied to the plant. The actuator
  7 % is equipped with a buffer where the control input sequence with the
  8 % most recent time stamp is stored. The actuator applies the control 
  9 % input of that buffered sequence that corresponds to the current time 
 10 % step. If there is no control input that corresponds to the current 
 11 % time step, a configurable default value is applied.
 12 %
 13 % REMARK: the function DataInDataOut has to be executed once every time
 14 % step
 15 %
 16 % AUTHOR: Jörg Fischer
 17 % LAST UPDATE: Maxim Dolgov, 03.05.2013
 18 %==========================================================================
 19 
 20 properties (SetAccess = private)
 21   %% input properties
 22     % length of buffer; (positive integer)
 23     packetLength = -1;
 24     % maximum allowed packet delay
 25     maxPacketDelay = -1;
 26     % dimension of control input; (positive integer)
 27     dimU = -1;
 28     % output value in case the buffer runs empty; (float)
 29     defaultU = 0;
 30   %% derived properties
 31     % buffer to store active control input sequence; (ClassDataPacket)
 32     bufferedPacket = [];
 33     % current time step; (positive integer)
 34     k = -1;
 35 
 36     debugInfo= [];
 37 end % properties
 38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 39 methods
 40   %% NCS_ClassActuator
 41   function s = NCS_ClassActuator(packetLength,maxPacketDelay,defaultU)
 42   %========================================================================
 43   % NCS_ClassActuator: creates an actuator object of type
 44   % (NCS_ClassActuator) and returns a handle to it
 45   %
 46   % Synopsis:
 47   % [s] = ClassActuator(packetLength,maxPacketDelay,dimU,defaultU);
 48   %
 49   % Input Paramter:
 50   % - packetLength: length of the actuator buffer; (positive integer)
 51   % - maxPacketDelay: maximum allowed delay experienced by transmitted
 52   %                   control sequences, may be Inf; 
 53   %                   (zero, positive integer or Inf)
 54   % - defaultU: default control input for the case the buffer runs empty;
 55   %             (column vector of dimension <dimU>)
 56   %
 57   % Output Paramter:
 58   % - s: (ClassActuator) initialized actuator
 59   %========================================================================
 60   
 61     % -------- 0) check input parameters
 62     s.ProcessInputs(packetLength,maxPacketDelay,defaultU);
 63 
 64     % -------- 1) initialize parameters
 65     s.bufferedPacket = NCS_ClassDataPacket();
 66     s.k = 1;
 67   end % function NCS_ClassActuator
 68 
 69   %% DataInDataOut
 70   function [uOut, markovMode] = DataInDataOut(s, numPackets, packetsIn)
 71   %========================================================================
 72   % DataInDataOut: Processing input and output data. To every 
 73   % function call 1) new data packets containing control input 
 74   % sequences are processed and if a packet with a more recent
 75   % time stamp is received than the control input sequence stored in 
 76   % the buffer, the new sequence is taken into the buffer.
 77   % 2) Output of the actuator is the control input of the buffered 
 78   % sequence that corresponds to the actual time step. If there is no
 79   % corresponding control input, the default control input determined
 80   % by defaultU is applied.
 81   %
 82   % Synopsis: 
 83   % [uOut, markovMode] = DataInDataOut(numPackets, packetsIn)
 84   %
 85   % Input Paramter:
 86   % - numPackets: number of control input sequences that 
 87   %   are received by the actuator in the current time step;
 88   %   (positive integer)
 89   % - packetsIn: is one or more data packet(s) that 
 90   %   is (are) received by the actuator at the current time step; The
 91   %   data packet contains a matrix with control inputs whereby the i-th
 92   %   column of the matrix contains a vector with the control inputs
 93   %   to be applied to the (timestamp of packet + i)-th time step
 94   %   (ClassDataPacket with property value is a matrix with dimension
 95   %   s.dimU x s.packetLength)
 96   %
 97   % Output Paramter:
 98   % - uOut: output of the actuator; (vector of dimension s.dimU)
 99   % - markovMode: mode of the Markov chain; (integer)
100   %
101   % REMARK: this function has to be executed once every time step      
102   %========================================================================
103 
104     % ------- 0) Check input parameters
105     % numPackets
106     if(~isscalar(numPackets))
107       error(['** Input parameter <numPackets> (number of received ',...
108              'packets) must be a scalar **'])
109     elseif(numPackets < 0) || ...
110           ((numPackets - floor(numPackets)) ~= 0)
111       error(['** Input parameter <numPackets> (number of received ',...
112              'packets) must be a positive integer **'])
113     end % check numPackets
114     % packetsIn
115     if(numPackets ~= 0)
116       for i = 1 : numPackets
117         if(~isa(packetsIn(i), 'NCS_ClassDataPacket'))
118           error(['** Input parameter <packetIn> (received packet) is', ...
119                  ' not of class NCS_ClassDataPacket **'])
120         end
121         if(any(isnan(packetsIn(i).value))) 
122           error(['** Input parameter <packetIn> (received packet) ',...
123                  'contains NaN **'])
124         elseif(~isequal(size(packetsIn(i).value), [s.dimU, s.packetLength]))
125           error(['** Dimension mismatch between control sequence in ',...
126                  ' <packetsIn> (received packet) and <dimU> ',...
127                  '(dimension of the control input) and <packetLength> ',...
128                  '(control sequence length) **'])
129         end
130       end
131     end % check packetsIn
132     
133     % ------- 1) Input data processing 
134     s.debugInfo{1, s.k} = s.k;
135     % NewPacketarrived?
136     if numPackets == 0
137       s.debugInfo{2, s.k} = NaN;
138       s.debugInfo{3, s.k} = NaN;
139     else
140       for i = 1 : numPackets
141         % new packet has arrived
142         if(((packetsIn(i).timeStamp + s.packetLength) > s.k) &&...
143             (packetsIn(i).timeStamp > s.bufferedPacket.timeStamp) && ...
144             (packetsIn(i).packetDelay <= s.maxPacketDelay))
145           % hold new packet in Buffer
146           s.bufferedPacket = packetsIn(i);
147         end
148         s.debugInfo{i * 2, s.k} = packetsIn(i).value;
149         s.debugInfo{i * 2 + 1, s.k} = packetsIn(i).timeStamp;
150       end
151     end
152     
153     % ------- 2) Output data processing 
154     % Data in buffer?
155     if(((s.bufferedPacket.timeStamp + s.packetLength) > s.k) && ...
156         (s.bufferedPacket.timeStamp > 0))
157       uOut = s.bufferedPacket.value(:,s.k -s.bufferedPacket.timeStamp + 1);
158       markovMode = s.k - s.bufferedPacket.timeStamp + 1;
159     else
160       % if buffer is empty than output default output
161       uOut = s.defaultU;
162       markovMode = s.packetLength+1;
163     end
164     % debug info
165     if numPackets == 0
166       s.debugInfo{4, s.k} = '----- Buffered:';
167       s.debugInfo{5, s.k} = s.bufferedPacket.value;
168       s.debugInfo{6, s.k} = s.bufferedPacket.timeStamp;
169       s.debugInfo{7, s.k} = '------ Output:';
170       s.debugInfo{8, s.k} = uOut;
171     else
172       s.debugInfo{(numPackets + 1) * 2, s.k} = '----- Buffered:'; 
173       s.debugInfo{(numPackets + 1) * 2 + 1, s.k} = ...
174         s.bufferedPacket.value;
175       s.debugInfo{(numPackets + 1) * 2 + 2, s.k} = ...
176         s.bufferedPacket.timeStamp;
177       s.debugInfo{(numPackets + 1) * 2 + 3, s.k} = '----- Output:'; 
178       s.debugInfo{(numPackets + 1) * 2 + 4, s.k} = uOut;
179     end
180     % ------- 3) Increment time step
181     s.k = s.k + 1;
182   end % function DataInDataOut
183 
184   %% Reset
185   function Reset(s)
186   %========================================================================
187   % Reset: resets the object
188   %
189   % Synopsis:
190   % Reset();
191   % 
192   % Input Paraemter:
193   % -- none --
194   %
195   % Output Parameter: 
196   % -- none --
197   %========================================================================
198 
199     % set defaults
200     s.bufferedPacket = NCS_ClassDataPacket();
201     s.k = 1;    
202   end % function Reset
203 end % methods public
204 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
205 methods (Access = private)
206   %% ProcessInputs
207   function ProcessInputs(s,packetLength,maxPacketDelay,defaultU)
208   %========================================================================
209   % ProcessInputs: checks the initialization parameters and assigns values
210   % to class properties
211   %
212   % Synopsis:
213   % ProcessInputs(packetLength,maxPacketDelay,defaultU)
214   % 
215   % Input Paramter:
216   % - packetLength: length of the actuator buffer; (positive integer)
217   % - maxPacketDelay: maximum allowed delay experienced by transmitted
218   %                   control sequences, max be Inf; 
219   %                   (zero, positive integer or Inf)
220   % - defaultU: default control input for the case the buffer runs empty;
221   %             (column vector of dimension <dimU>)
222   % 
223   % Output Parameters:
224   % -- none --
225   %========================================================================
226   
227     % packetLength
228       if (~isscalar(packetLength))
229         error(['** Input parameter <packetLength> (control sequence ',...
230                'packet length) must be a scalar **'])
231       elseif (packetLength <= 0)
232         error(['** Input parameter <packetLength> (control sequence ',...
233                'packet length) must be positive **'])
234       else
235         s.packetLength = packetLength;
236       end % packetLength
237 
238     % maxPacketDelay
239       if(isnan(maxPacketDelay))
240         error(['** Input parameters <maxPacketDelay> (maximum allowed ',...
241                  'packet delay) is NaN **'])
242       elseif(~isscalar(maxPacketDelay))
243         error(['** Input parameters <maxPacketDelay> (maximum allowed ',...
244                  'packet delay) is not a scalar **'])
245       elseif((maxPacketDelay-floor(maxPacketDelay)) ~= 0 && maxPacketDelay ~= Inf || ...
246              maxPacketDelay < 0)
247         error(['** Input parameters <maxPacketDelay> (maximum allowed ',...
248                  'packet delay) is not a positive integer or zero **'])
249       else
250         s.maxPacketDelay = maxPacketDelay;
251       end % maxPacketDelay
252 
253     % defaultU
254       if(size(defaultU,2) ~= 1 || size(defaultU,1) < 1)
255         error(['** Input parameters <defaultU> (default input) must ',...
256                'be a column vector or a scalar **'])
257       elseif(any(isnan(defaultU)))
258         error(['** Input parameters <defaultU> (default input) ',...
259                'contains NaN **'])
260       else
261         s.defaultU = defaultU;
262         s.dimU = size(defaultU,1);
263       end % defaultU
264   end % function ProcessInputs
265 end % methods private
266 end % classdef
Download algorithm (11 files) as ZIP

Comments

Please login to post a comment.