CloudRunner

... share your algorithms as a web service

Algorithm: Optimal Sequence-Based LQG Control over TCP-Like Networks

Description:
This function performs a Monte Carlo simulation of the optimal
sequence-based LQG controller proposed in J. Fischer, A. Hekler, M.
Dolgov, and U. Hanebeck "Optimal Sequence-Based LQG Control over TCP-like
Networks Subject to Random Transmission Delays and Packet Losses".

In the simulated scenario the controller drives a disturbed double
integrator ... (show more)
Paper: Jörg Fischer, Achim Hekler, Maxim Dolgov, and Uwe D. Hanebeck. Optimal sequence-based lqg control over tcp-like networks subject to random transmission delays and packet losses (to appear). In Proceedings of the 2013 American Control Conference (ACC 2013). Washington D. C., USA, June 2013.
Tags: LQG TCP sequence-based control NCS
Usage: Algorithm is public.
Viewed 2671 times, called 46 times
Upload:
Empty star Empty star Empty star Empty star Empty star
0 votes
Maxim Dolgov
06/14/2013 4:15 p.m. (version #1)

Run Algorithm

:
ID of the simulated network
(ID = 1: "good" network)
(ID = 2: "bad" network)
: simulation time per Monte Carlo run
: number of Monte Carlo runs
(the higher, the better but the longer the computation time)
: disturbance of the system
:
packet length used for the example trajectory
: Allow cached result?
Close

Please Wait

Computation is running...

Plots

showing plot # of

Result Value(s)


		

Resource Usage

Execution of the algorithm took seconds.
Peak memory usage for running the algorithm was kilobytes (total usage: kilobytes).

Run ID / Link

ID of this run:
Link:

Result from Cache

Result has been delivered from cache and computed on (UTC).

Matlab log

(show)

		

Error

Using this algorithm in your local MATLAB environment is easy: Click here for instructions!

Usage Instructions for CloudRunner Client

  1. Download the CloudRunner Client by clicking here and place the downloaded file in your MATLAB working directory.

  2. Inside MATLAB, initialize the CloudRunner Client by calling CloudRunner:
    >> CloudRunner

    A login dialog will prompt for your CloudRunner mail address and password. For a start, you can leave the dialog empty and just click "Connect".

    Alternatively, you can provide the login credentials (or empty strings to skip login) as a parameter and hence skip the login dialog. This is useful when using CloudRunner in non-interactive scripts.
    >> CloudRunner('mail@example.com', 'password')

  3. Select this algorithm by its URL. Selecting an algorithm creates the lambda function that proxies calls to the algorithm to the server for execution:
    >> CloudRunnerSelect('http://www.cloudrunner.eu/algorithm/142/optimal-sequence-based-lqg-control-over-tcp-like-networks/version/1/')

    For the sake of convenience, you can also use the algorithm ID instead of its URL for public algorithms.

  4. Call functions from the algorithm like any regular local function.

Note: You can find further information on the help page.

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 classdef NCS_ClassTcpNcs < handle
  2 %==========================================================================
  3 % NCS_ClassTcpNcs: implementation of a complete control loop containing an
  4 % optimal TCP-controller (NCS_ClassTcpController), a controller-actuator
  5 % network (ClassCommNetwork), an actuator (NCS_ClassActuator) which accepts
  6 % delayed control sequences, a linear plant, a linear sensor, a
  7 % senor-controller network (ClassCommNetwork) and a state estimator
  8 % (ClassDelayedKF). This class performs a single simulation run with
  9 % <simTime> time steps and returns cumulated costs.
 10 %
 11 %
 12 % AUTHOR: Maxim Dolgov
 13 % LAST UPDATE: Maxim Dolgov, 12.06.2013
 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   %% derived properties and components
 60     % dimension of the system state; (positive integer)
 61     dimX = -1;
 62     % dimension of the control input; (positive integer)
 63     dimU = -1;
 64     % dimension of the measruement; (positive integer)
 65     dimY = -1;
 66     % handle to the controller; (NCS_ClassTcpController)
 67     controller = [];
 68     % handle to the controller-actuator network; (NCS_ClassCommNetwork)
 69     caNetwork = [];
 70     % handle to the controller-actuator data packet; (NCS_ClassDataPacket)
 71     caPacket = [];
 72     % handle to the actuator; (NCS_ClassActuator)
 73     actuator = [];
 74     plant = [];
 75     sensor = [];
 76     scNetwork = [];
 77     scPacket = [];
 78     estimator = [];
 79 end % properties
 80 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 81 methods 
 82   %% NCS_ClassTcpNcs
 83   function s = NCS_ClassTcpNcs(A,B,C,Q,R,x0Mean,x0Cov,wCov,vCov,simTime, ...
 84                            caDelayProb,caPacketLength,scMaxDelay)
 85   %========================================================================
 86   % NCS_ClassTcpNcs: initializes a new object of type <NCS_ClassTcpNcs> and returns
 87   % a handle to it
 88   % 
 89   % Synopsis:
 90   %
 91   % Input Parameters:
 92   % 
 93   % Output Parameters:
 94   %========================================================================
 95   
 96   % The inputs will be checked by the components in this implementation.
 97   % Future implementations will check the inputs also here.
 98 %   s.ProcessInputs(A,B,C,Q,R,x0Cov,wCov,vCov,x_0,simTime,v,w, ...
 99 %                   caDelayProb,caPacketLength);
100     s.A = A;
101     s.dimX = size(A,1);
102     s.B = B;
103     s.dimU = size(B,2);
104     s.C = C;
105     s.dimY = size(C,1);
106     s.Q = Q;
107     s.R = R;
108     s.x0Cov = x0Cov;
109     s.wCov = wCov;
110     s.vCov = vCov;
111 %     s.x_0 = x_0;
112     s.simTime = simTime;
113     s.caDelayProb = caDelayProb;
114     s.caPacketLength = caPacketLength;
115     s.scMaxDelay = scMaxDelay;
116     s.x0Mean = x0Mean;
117   
118 	% initialize components
119     s.controller = NCS_ClassTcpController(s.A,s.B,s.Q,s.R,s.caDelayProb,...
120                                           s.caPacketLength);
121     s.caNetwork = NCS_ClassCommNetwork(s.simTime,s.caPacketLength+1);
122     s.actuator = NCS_ClassActuator(s.caPacketLength,Inf,zeros(s.dimU,1));
123     s.plant = NCS_ClassLinearPlant(s.A,s.B,zeros(s.dimX,1));
124     s.sensor = NCS_ClassLinearSensor(s.C,s.dimX);
125     s.scNetwork = NCS_ClassCommNetwork(simTime,s.scMaxDelay);
126   end % function NCS_ClassTcpNcs
127   %% Simulate
128   function results = Simulate(s,x_0,w,v,caPacketDelays,scPacketDelays)
129   %========================================================================
130   % Simulate: performs a simulation run
131   %========================================================================
132   
133   % reset reusable components
134   s.controller.Reset();
135   s.actuator.Reset();
136   s.plant.Reset(x_0);
137   s.sensor.Reset();
138   s.caNetwork.Reset();
139   s.caPacket = NCS_ClassDataPacket();
140   s.scNetwork.Reset();
141   s.scPacket = NCS_ClassDataPacket();
142   % initialize dynamic components
143   s.estimator = NCS_ClassDelayedKF(s.A,s.B,s.C,s.wCov,s.vCov,s.x0Mean,s.x0Cov,...
144                                s.scMaxDelay,s.caDelayProb,s.caPacketLength);
145   
146   % construct output structure
147   results.realTrajectory = zeros(s.dimX,s.simTime+1);
148   results.realTrajectory(:,1) = x_0;
149   results.estimatedTrajectory = zeros(s.dimX,s.simTime+1);
150   results.estimatedTrajectory(:,1) = x_0;
151   results.appliedU = zeros(s.dimU,s.simTime);
152   
153   % simulation variables
154   xOut = x_0;
155   mode = s.caPacketLength+1;
156   % main simulation loop
157   for k = 1 : s.simTime
158     s.caPacket = s.controller.DataInDataOut(xOut,mode);
159     [numArrivedCaPackets,arrivedCaPackets] = ...
160                                  s.caNetwork.DataInDataOut(s.caPacket,caPacketDelays(k));
161     [uOut,mode] = ...
162             s.actuator.DataInDataOut(numArrivedCaPackets,arrivedCaPackets);
163     xReal = s.plant.DataInDataOut(uOut,w(:,k));
164       
165     s.scPacket = s.sensor.DataInDataOut(xReal,v(:,k));
166     
167     [numArrivedScPackets,arrivedScPackets] = ...
168                                  s.scNetwork.DataInDataOut(s.scPacket,scPacketDelays(k));
169                                
170     xOut = s.estimator.DataInDataOut(s.caPacket,numArrivedScPackets,...
171                                      arrivedScPackets);
172 
173     % store simulation results of this step
174     results.realTrajectory(:,k+1) = xReal;
175     results.estimatedTrajectory(:,k+1) = xOut;
176     results.appliedU(:,k) = uOut;
177   end % main simulation loop
178   % compute and return costs
179   results.costs = s.ComputeCosts(results.realTrajectory,results.appliedU);
180   end % function Simulate
181 end % methods public
182 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
183 methods (Access = private)
184 %   %% ProcessInputs
185 %   function ProcessInputs(s,A,B,C,Q,R,x0Cov,wCov,vCov,x_0,simTime,v,w, ...
186 %                          caDelayProb,caPacketLength)
187 %   %========================================================================
188 %   % ProcessInputs: analyzes and stores input parameters
189 %   % 
190 %   % Synopsis: 
191 %   % ProcessInputs(A,B,C,Q,R,x0Cov,wCov,vCov,x_0,simTime,v,w, ...
192 %   %               caDelayProb,caPacketLength);
193 %   % 
194 %   % Input Parameters:
195 %   %
196 %   % Output Parameters:
197 %   % -- none --
198 %   %========================================================================
199 %   
200 %     % A
201 %     if(size(A,1) ~= size(A,2))
202 %       size(A)
203 %       error('** Input parameter <A> (system matrix) must be square **')
204 %     else
205 %       s.A = A;
206 %       s.dimX = size(s.A,1);
207 %     end % A
208 %     
209 %     % B
210 %     if(size(B,1) ~= size(s.A,1))
211 %       error(['** Dimension mismatch between input parameters <A> ',...
212 %              '(system matrix) and <B> (input matrix) **'])
213 %     else
214 %       s.B = B;
215 %       s.dimU = size(s.B,2);
216 %     end % B
217 %     
218 %      % C
219 %     if(size(C,2) ~= s.dimX)
220 %       error(['** Dimension mismatch between input parameters <A> ',...
221 %              '(system matrix) and <C> (measurement matrix) **'])
222 %     else
223 %       s.C = C;
224 %       s.dimY = size(s.C,1);
225 %     end % C
226 %     
227 %     % Q
228 %     if(size(Q,1) ~= size(Q,2))
229 %       size(Q)        
230 %       error(['** Input parameter <Q> (system state cost matrix) must',...
231 %              'be square **'])
232 %     elseif(size(s.A,1) ~= size(Q,1))
233 %       error(['** Dimension mismatch between input parameters <A> ',...
234 %              '(system matrix) and <Q> (system state cost matrix) **'])
235 %     elseif(sum(sum(Q ~= Q')))
236 %       error(['** Input parameter <Q> (system state cost matrix) must',...
237 %              'be symmetric **'])
238 %     elseif(sum(eig(Q) < 0))
239 %      error(['** Input parameter <Q> (system state cost matrix) must',...
240 %             'be positive semidefinite **'])
241 %     elseif(any(isnan(B)))
242 %       error(['** Input parameter <Q> (system state cost matrix) ',...
243 %              'contains NaN **'])
244 %     else
245 %       s.Q = Q;
246 %     end % Q
247 %     
248 %     % R
249 %     if(size(R,1) ~= size(R,2))
250 %       error(['** Input parameter <R> (input cost matrix) must',...
251 %              'be square **'])
252 %     elseif(size(B,2) ~= size(R,1))
253 %       error(['** Dimension mismatch between input parameters <B> ',...
254 %              '(input matrix) and <R> (input cost matrix) **'])
255 %     elseif(sum(sum(R ~= R')))
256 %       error(['** Input parameter <R> (input cost matrix) must',...
257 %              'be symmetric **'])
258 %     elseif(sum(eig(R) <= 0))
259 %      error(['** Input parameter <R> (input cost matrix) must',...
260 %             'be positive definite **'])
261 %     elseif(any(isnan(R)))
262 %       error('** Input parameter <R> (input cost matrix) contains NaN **')
263 %     else
264 %       s.R = R;
265 %     end % R
266 %     
267 %     % caPacketLength
268 %     if(~isscalar(caPacketLength))
269 %       error(['** Input parameter <caPacketLength> (control sequence ',...
270 %              'packet length) must be a scalar **'])
271 %     elseif(caPacketLength <= 0)
272 %       error(['** Input parameter <caPacketLength> (control sequence ',...
273 %              'packet length) must be positive **'])
274 %     elseif(any(isnan(caPacketLength)))
275 %       error(['** Input parameter <caPacketLength> (control sequence ',...
276 %              'packet length) contains NaN **'])
277 %     else
278 %       s.caPacketLength = caPacketLength;
279 %     end % caPacketLength
280 %       
281 %     % caDelayProb
282 %     if(min(size(caDelayProb)) ~= 1)
283 %       error(['** Input parameter <caDelayProb> (packet delay ',...
284 %              'probability distribution) must be a vector **'])
285 %     elseif(numel(caDelayProb) < s.caPacketLength)
286 %       error(['** Input parameter <caDelayProb> (packet delay ',...
287 %              'probability distribution) is too short **'])
288 %     elseif(abs(sum(caDelayProb)-1) > 1e-6)
289 %       error(['** Sum of elements in the input parameter <caDelayProb>',...
290 %              ' (packet delay probability distribution) must be 1 **'])
291 %     elseif(any(isnan(caDelayProb)))
292 %       error(['** Input parameter <caDelayProb> (packet delay ',...
293 %              'probability distribution) contains NaN **'])
294 %     else
295 %       if(numel(s.caDelayProb) == s.caPacketLength)
296 %         tmpDelayProb(end+1) = 0;
297 %       else
298 %         tmpDelayProb = zeros(1,s.caPacketLength+1);
299 %         tmpDelayProb(1:s.caPacketLength) = delayProb(1:s.caPacketLength);
300 %         tmpDelayProb(end) = sum(caDelayProb(s.caPacketLength+1:end));
301 %       end
302 %       s.caDelayProb = tmpDelayProb;
303 %     end % caDelayProb
304 %       
305 %     % x_0
306 %     if(min(size(x_0)) ~= 1)
307 %       error(['** Input parameter <x_0> (initial system state)',...
308 %              ' must be a vector **'])
309 %     elseif(length(x_0) ~= size(s.A,1))
310 %       error(['** Dimension mismatch between input parameters ',...
311 %              '<x_0> (initial system state) and <A> (system matrix) **'])
312 %     else
313 %       s.x_0 = x_0;
314 %     end % x_0
315 %   end % function ProcessInputs
316 
317   %% ComputeCosts
318   function costs = ComputeCosts(s,xTrajectory,uSeq)
319   %========================================================================
320   % ComputeCosts: computes average costs per time step according to an
321   % LQG cost function
322   %
323   % Synopsis:
324   % costs = ComputeCosts(xTrajectory,uSeq);
325   % 
326   % Input Parameters: 
327   % - xTrajectory: state trajectory; 
328   %                (matrix of dimension <dimX> x <simTime+1>)
329   % - uSeq: sequence of applied control inputs;
330   %         (matrix of dimension <dimU> x <simTime>)
331   %
332   % Output Parameters::
333   % - costs: average costs per time step; (positive integer)
334   %========================================================================
335   
336   costs = 0;
337     for i = 1:s.simTime
338       costs = costs + ...
339               xTrajectory(:,i)'*s.Q*xTrajectory(:,i) + ...
340               uSeq(:,i)'*s.R*uSeq(:,i);
341     end
342     costs = costs + ...
343       xTrajectory(:,s.simTime+1)'*s.Q*xTrajectory(:,s.simTime+1);
344     costs = costs/s.simTime;
345   end % function ComputeCosts
346 end % methods private
347 end % classdef
  1 classdef NCS_ClassTcpController < handle
  2 %==========================================================================
  3 % NCS_ClassTcpController: optimal linear controller for NCS with a TCP-like
  4 % network connecting the controller and the actuator. The controller is
  5 % described in J. Fischer, A. Hekler, M. Dolgov, U. Hanebeck, 
  6 % "Optimal Sequence-Based LQG Control over TCP-like Networks 
  7 % Subject to Random Transmission Delays and Packet Losses"
  8 % <a href="matlab:web('http://arxiv.org/abs/1211.3020')">[link]</a>.
  9 %
 10 % NCS_ClassTcpController( A , B , Q , R , delayProb , packetLength )
 11 % 
 12 % dataPacket = DataInDataOut( x , markovMode )
 13 %
 14 % REMARK: DataInDataOut must be executed once every simulation time step
 15 % REMARK: no support for default control input yet
 16 %
 17 % TODO: make "MakeSteadyStateControlCovarianceMatrices" without "tmpP"
 18 % 
 19 % AUTHOR: Maxim Dolgov
 20 % LAST UPDATE: Maxim Dolgov, 15.04.2013
 21 %==========================================================================
 22 properties (SetAccess = private)
 23   %% input properties
 24     % state matrix; (matrix of dimension <dimX> x <dimX>)
 25     A = [];
 26     % input matrix; (matrix of dimension <dimX> x <dimU>)
 27     B = [];
 28     % system state cost matrix;
 29     % (positive semidefinite matrix of dimension <dimX> x <dimX>)
 30     Q = [];
 31     % input cost matrix;
 32     % (positive definite matrix of dimension <dimU> x <dimU>)
 33     R = [];
 34     % packet delay probability density function of the controller-actuator-
 35     % link; (vector of dimension >= <packetLength>)
 36     delayProb = [];
 37     % control sequence length; (positive integer)
 38     packetLength = -1;
 39   %% derived properties
 40   % CA-network-actuator system
 41     % mapping for propagation of possible control inputs;
 42     % (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x 
 43     %  <dimU*packetLength*(packetLength-1)/2>)
 44     F = [];
 45     % matrix mapping control sequence to the possible control inputs;
 46     % (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x
 47     %  <dimU*packetLength>)
 48     G = [];
 49   % augmented sate space
 50     % system state of the augmented state space;
 51     % (column vector of dimension <dimState>)
 52     sysState = [];
 53     % system matrix of the augmented state space;
 54     % (matrix of dimension <dimState> x <dimState>)
 55     augA = [];
 56     % control matrix of the augmented state space;
 57     % (matrix of dimension <dimState> x <dimU*packetLength>)
 58     augB = [];
 59     % state cost matrix of the augmented state space;
 60     % (matrix of dimension <dimState> x <dimState>)
 61     augQ = [];
 62     % control sequence cost matrix of the augmented state space;
 63     % (matrix of dimension <dimU*packetLength> x <dimU*packetLength>)
 64     augR = [];
 65 	% Markov chain
 66     % transition matrix of the Markov chain;
 67     % (matrix of dimension <packetLength+1> x <packetLength+1>)
 68     transitionMatrix = [];
 69     % current mode of the markov chain; (positive integer)
 70     markovMode = -1;
 71 	% controller
 72     % control covariance matrix; 
 73     % (matrix of dimension <dimState> x <dimState>)
 74     P = [];
 75     % control gain matrix: U = -L*sysState;
 76     % (matrix of dimension <dimU*packetLength> x <dimState>)
 77     L = [];
 78   % dimensions
 79     % dimension of the system state; (positive integer)
 80     dimX = -1;
 81     % dimension of the control input; (positive integer)
 82     dimU = -1;
 83     % dimension of the augmented system state; 
 84     % (positive integer <dimX+dimU*packetLength*(packetLength-1)/2>)
 85     dimState = -1;
 86   % system time; (positive integer)
 87     k = -1;
 88   %% debugging properties
 89     debugging = [];
 90 end % properties
 91 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 92 methods
 93   %% NCS_ClassTcpController
 94   function s = NCS_ClassTcpController(A,B,Q,R,delayProb,packetLength)
 95   %========================================================================
 96 	% NCS_ClassTcpController: creates and returns a handle to the object of
 97 	% type (NCS_ClassTcpController)
 98   %
 99   % Synopsis:
100   % [s] = NCS_ClassTcpController(A,B,Q,R,delayProb,packetLength)
101   %
102   % Input Parameter:
103   % - A: state matrix; (matrix of dimension <dimX> x <dimX>)
104   % - B: input matrix; (matrix of dimension <dimX> x <dimU>)
105   % - Q: system state cost matrix;
106   %      (positive semidefinite matrix of dimension <dimX> x <dimX>)
107   % - R: input cost matrix;
108   %      (positive definite matrix of dimension <dimU> x <dimU>)
109   % - delayProb: packet delay probability density function of the 
110   %              controller-actuator- link; (vector of dimension )
111   % - packetLength: control sequence length; (positive integer)
112   %
113   % Output Parameter:
114   % - s: handle to the object created object; (NCS_ClassTcpController)
115   %========================================================================
116   
117     % process and store input parameters
118     s.ProcessInputs(A,B,Q,R,delayProb,packetLength);
119 
120     % augmented initial system state
121       if(s.packetLength == 1)
122         s.sysState = zeros(s.dimX, 1);
123       else
124         s.sysState = zeros(s.dimX + ...
125                              s.dimU*(s.packetLength*(s.packetLength-1)/2),1);
126       end % initial augmented system state
127       s.dimState = length(s.sysState);
128       
129     % augmented state space matrices
130       s.F = s.MakeFMatrix(s.packetLength,s.dimU);
131       s.G = s.MakeGMatrix(s.packetLength,s.dimU);
132       H = s.MakeHMatrices(s.packetLength,s.dimU);
133       J = s.MakeJMatrices(s.packetLength,s.dimU);
134     [s.augA , s.augB] = ...
135           s.MakeAugmentedStateMatrices(s.A,s.B,s.F,s.G,H,J,s.packetLength);
136     [s.augQ , s.augR] = ...
137                    s.MakeAugmentedCostMatrices(s.Q,s.R,H,J,s.packetLength);
138       
139     % initialize Markov chain properties
140     s.markovMode = s.packetLength+1;
141     s.transitionMatrix = s.MakeTransitionMatrix(s.delayProb, ...
142                                                            s.packetLength);  
143                                                          
144     % make controller covariance matrices
145     s.P = s.MakeSteadyStateControlCovarianceMatrices();
146     s.L = s.MakeSteadyStateControlGainMatrices();
147     
148     % set system time
149     s.k = 1;
150   end % function NCS_ClassTcpController    
151   
152   %% DataInDataOut
153   function packet = DataInDataOut(s,x,mode)
154   %========================================================================
155 	% DataInDataOut: generates a control input sequence based on the estimate
156   % of the current system state and the Markov mode in the previous time
157   % step. The calculated control sequence is dispatched in a data packet
158   % (NCS_ClassDataPacket).
159   % 
160   % Synopsis:
161   % [packet] = DataInDataOut(x, mode)
162   % 
163   % Input Parameter:
164   % - x: current system state; (column vector of dimension <dimX>)
165   % - mode: previous Markov mode; (positive integer or zero)
166   %
167   % Output Parameter:
168   % - packet: data packet containing the control sequence; 
169   %           (NCS_ClassDataPacket)
170   %
171   % REMARK: this function must be executed once every time step
172   %========================================================================
173     
174     % check inputs
175     % x
176       if(size(x,2) ~= 1)
177         error(['** Input parameter <x> (current system state) is not a ',...
178                'column vector **'])
179       elseif(size(x,1) ~= s.dimX)
180         error(['** Input parameter <x> (current system state) is not a ',...
181                'has wrong dimension **'])
182       elseif(any(isnan(x)))
183         error(['** Input parameter <x> (current system state) contains ',...
184                'NaN **'])
185       end % x
186     % mode
187       if(~isscalar(mode))
188         error(['** Input parameter <mode> (current Markov mode) is ',...
189                'not a scalar **'])
190       elseif(mode-floor(mode) ~= 0)
191         error(['** Input parameter <mode> (current Markov mode) is ',...
192                'not an integer **'])
193       elseif((mode < 1) || mode > s.packetLength+1)
194         error(['** Input parameter <mode> (current Markov mode) exceeds',...
195                'allowed interval **'])
196       end % mode
197   
198     % update augmented system state 
199     s.sysState(1:s.dimX) = x;
200     % store previous Markov mode
201     s.markovMode = mode;
202     
203     % generate control sequence
204     controlSequence = s.L(:,:,mode)*s.sysState;
205     packet = NCS_ClassDataPacket(reshape(controlSequence,s.dimU,s.packetLength),...
206                              s.k);
207 
208     % update possible control inputs
209     s.sysState(s.dimX+1:end) = s.F*s.sysState(s.dimX+1:end) + ...
210                                s.G*controlSequence;
211     s.k = s.k + 1;
212   end % function DataInDataOut
213   
214   %% Reset
215   function Reset(s)
216   %========================================================================
217 	% Reset: puts the controller into initial state
218   % 
219   % Synopsis:
220   % Reset()
221   % 
222   % Input Parameter:
223   % -- none --
224   %
225   % Output Parameter:
226   % -- none --
227   %========================================================================
228   
229     s.markovMode = s.packetLength+1;
230     s.sysState = zeros(s.dimState,1);
231     s.k = 1;
232   end % function Reset
233   
234 end % methods public
235 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
236 methods (Access = private)
237   %% ProcessInputs
238   function ProcessInputs(s,A,B,Q,R,delayProb,packetLength)
239 	%========================================================================
240 	% ProcessInputs: analyzes and stores provided class parameters
241   %
242   % Synopsis:
243   % ProcessInputs(A, B, Q, R, delayProb, packetLength)
244   %
245   % Input Parameter:
246   % - A: state matrix; (matrix of dimension <dimX> x <dimX>)
247   % - B: input matrix; (matrix of dimension <dimX> x <dimU>)
248   % - Q: system state cost matrix;
249   %      (positive semidefinite matrix of dimension <dimX> x <dimX>)
250   % - R: input cost matrix;
251   %      (positive definite matrix of dimension <dimU> x <dimU>)
252   % - delayProb: packet delay probability density function of the 
253   %              controller-actuator- link; (vector of dimension )
254   % - packetLength: control sequence length; (positive integer)
255   %
256   % Output Parameter:
257   % -- none --
258   %========================================================================
259   
260     % A
261     if(size(A,1) ~= size(A,2))
262       size(A)
263       error('** Input parameter <A> (system matrix) must be square **')
264     elseif(any(isnan(A)))
265       error('** Input parameter <A> (system matrix) contains NaN **')
266     else
267       s.A = A;
268       s.dimX = size(s.A,1);
269     end % A
270     
271     % B
272     if(size(B,1) ~= size(s.A,1))
273       error(['** Dimension mismatch between input parameters <A> ',...
274              '(system matrix) and <B> (input matrix) **'])
275     elseif(any(isnan(B)))
276       error('** Input parameter <B> (input matrix) contains NaN **')
277     else
278       s.B = B;
279       s.dimU = size(s.B,2);
280     end % B
281       
282     % Q
283     if(size(Q,1) ~= size(Q,2))
284       size(Q)        
285       error(['** Input parameter <Q> (system state cost matrix) must',...
286              'be square **'])
287     elseif(size(s.A,1) ~= size(Q,1))
288       error(['** Dimension mismatch between input parameters <A> ',...
289              '(system matrix) and <Q> (system state cost matrix) **'])
290     elseif(sum(sum(Q ~= Q')))
291       error(['** Input parameter <Q> (system state cost matrix) must',...
292              'be symmetric **'])
293     elseif(sum(eig(Q) < 0))
294      error(['** Input parameter <Q> (system state cost matrix) must',...
295             'be positive semidefinite **'])
296     elseif(any(isnan(B)))
297       error(['** Input parameter <Q> (system state cost matrix) ',...
298              'contains NaN **'])
299     else
300       s.Q = Q;
301     end % Q
302     
303     % R
304     if(size(R,1) ~= size(R,2))
305       error(['** Input parameter <R> (input cost matrix) must',...
306              'be square **'])
307     elseif(size(B,2) ~= size(R,1))
308       error(['** Dimension mismatch between input parameters <B> ',...
309              '(input matrix) and <R> (input cost matrix) **'])
310     elseif(sum(sum(R ~= R')))
311       error(['** Input parameter <R> (input cost matrix) must',...
312              'be symmetric **'])
313     elseif(sum(eig(R) <= 0))
314      error(['** Input parameter <R> (input cost matrix) must',...
315             'be positive definite **'])
316     elseif(any(isnan(R)))
317       error('** Input parameter <R> (input cost matrix) contains NaN **')
318     else
319       s.R = R;
320     end % R
321     
322     % packetLength
323     if(~isscalar(packetLength))
324       error(['** Input parameter <packetLength> (control sequence ',...
325              'packet length) must be a scalar **'])
326     elseif(packetLength <= 0)
327       error(['** Input parameter <packetLength> (control sequence ',...
328              'packet length) must be positive **'])
329     elseif(any(isnan(packetLength)))
330       error(['** Input parameter <packetLength> (control sequence ',...
331              'packet length) contains NaN **'])
332     else
333       s.packetLength = packetLength;
334     end % packetLength
335       
336     % delayProb
337     if(min(size(delayProb)) ~= 1)
338       error(['** Input parameter <delayProb> (packet delay ',...
339              'probability distribution) must be a vector **'])
340     elseif(numel(delayProb) < s.packetLength)
341       error(['** Input parameter <delayProb> (packet delay ',...
342              'probability distribution) is too short **'])
343     elseif(abs(sum(delayProb)-1) > 1e-6)
344       error(['** Sum of elements in the input parameter <delayProb>',...
345              ' (packet delay probability distribution) must be 1 **'])
346     elseif(any(isnan(delayProb)))
347       error(['** Input parameter <delayProb> (packet delay ',...
348              'probability distribution) contains NaN **'])
349     else
350       if(numel(s.delayProb) == s.packetLength)
351         tmpDelayProb(end+1) = 0;
352       else
353         tmpDelayProb = zeros(1,s.packetLength+1);
354         tmpDelayProb(1:s.packetLength) = delayProb(1:s.packetLength);
355         tmpDelayProb(end) = sum(delayProb(s.packetLength+1:end));
356       end
357       s.delayProb = tmpDelayProb;
358     end % delayProb
359   end % function ProcessInputs
360   
361   %% MakeSteadyStateControlCovarianceMatrices
362   function P = MakeSteadyStateControlCovarianceMatrices(s)
363 	%========================================================================
364 	% MakeSteadyStateControlCovarianceMatrices: computes the steady state
365 	% solution of the controller by setting the horizon length to infinity
366 	% (actually done by iterating through the horizon until the difference
367 	% between two consecutive iteration does not exceed <maxConsecutiveDiff>
368 	% defined after this comment).
369   %
370   % Synopsis:
371   % MakeSteadyStateControlCovarianceMatrices()
372   %
373   % Input Parameter:
374   % -- none --
375   % 
376   % Output Parameter:
377   % - P: 3D matrix with control covariances; 
378   %      (positive semidefinite matrix of dimension 
379   %       <dimState> x <dimState> x <packetLength+1>)
380   %
381   % P(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
382   %========================================================================  
383        
384     % maximum difference between two consecutive iterations
385     maxIterationDiff = 1e-4;
386   
387     P = zeros(s.dimState, s.dimState, s.packetLength+1);
388   
389     tmpP = P;            
390     previousP = P;
391     
392     % initialize P
393     for i = 1 : s.packetLength+1
394       P(1:s.dimX,1:s.dimX,i) = s.Q;
395     end % initialize P
396     
397     counter = 0; % counts the number of performed iterations
398     while(sum(sum(sum(abs(P - previousP)))) > maxIterationDiff)
399       counter = counter + 1;
400       previousP = P;
401       
402       for j = 1 : s.packetLength+1
403         % temporary components of
404         P1 = 0;
405         P2 = 0;
406         P3 = 0;
407         for i = 1 : s.packetLength + 1
408           P1 = P1 + s.transitionMatrix(j,i)*...
409                (s.augQ(:,:,i) + s.augA(:,:,i)'*P(:,:,i)*s.augA(:,:,i));
410           P2 = P2 + s.transitionMatrix(j,i)*...
411                (s.augA(:,:,i)'*P(:,:,i)*s.augB(:,:,i));
412           P3 = P3 + s.transitionMatrix(j,i)*...
413                (s.augR(:,:,i) + s.augB(:,:,i)'*P(:,:,i)*s.augB(:,:,i));
414         end % i: current Markov mode
415         tmpP(:,:,j) = P1 - P2*pinv(P3)*P2';
416       end % j: previus Markov mode
417       P = tmpP;
418     end % while    
419     s.debugging.numIterationsToSteadyStateP = counter;
420 %     disp('für P ',int2str(counter),' Iterationen')
421   end % function MakeSteadyStateControlCovarianceMatrices
422   
423   %% MakeSteadyStateControlGainMatrices
424   function L = MakeSteadyStateControlGainMatrices(s)
425   %========================================================================
426 	% MakeSteadyStateControlGainMatrices: computes the gain matrices which
427 	% are used to calculate the control sequence by multiplying with the
428 	% augmented system state: U = L*x
429   %
430   % Synopsis:
431   % MakeSteadyStateControlGainMatrices()
432   %
433   % Input Parameter: 
434   % -- none --
435   % 
436   % Output Parameter:
437   % - L: 3D matrix of control gains; (matrix of dimension 
438   %      <dimU*packetLength> x <dimState> x <packetLength+1>)
439   %
440   % L(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
441   %========================================================================  
442     
443     L = zeros(s.dimU*s.packetLength,s.dimState,s.packetLength+1);
444     
445     for j = 1:s.packetLength+1
446       L1 = 0;
447       L2 = 0;
448       for i = 1:s.packetLength+1
449         L1 = L1 + s.transitionMatrix(j,i)*...
450              (s.augR(:,:,i) + s.augB(:,:,i)'*s.P(:,:,i)*s.augB(:,:,i));
451         L2 = L2 + s.transitionMatrix(j,i)*...
452              (s.augB(:,:,i)'*s.P(:,:,i)*s.augA(:,:,i));
453       end % i: current Markov mode
454       L(:,:,j) = - pinv(L1)*L2;                   
455     end % i: previous Markov mode
456   end % function MakeSteadyStateControlGainMatrices
457   
458 end % methods private
459 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
460 methods (Static)
461   %% ComputetransitionMatrix
462   function T = MakeTransitionMatrix(delayProb,packetLength)
463   %========================================================================
464 	% MakeTransitionMatrix: computes the transition matrix of the Markov
465 	% chain which governs the packet arrival process at the actuator.
466   % 
467   % Synopsis:
468   % [T] = MakeTransitionMatrix(delayProb, packetLength)
469   % 
470   % Input Parameter:
471   % - delayProb: packet delay probability density function of the 
472   %              controller-actuator- link; (vector of dimension )
473   % - packetLength: control sequence length; (positive integer)
474   %
475   % Output Parameter:
476   % - T: transition matrix of the Markov chain
477   %
478   % T(i,j) = Prob(nextMode = j | currentMode = i)
479   %========================================================================
480   
481     % determine number of states of the Markov chain
482     numStates = packetLength+1;
483     
484     T = zeros(numStates,numStates);
485 
486     % calculate transition matix
487     for i = 1 : numStates
488       for j = 1 : numStates
489         if(j >= i +2)
490           T(i,j) = 0;
491         elseif(j == i + 1)
492           T(i,j) = 1 - (sum(delayProb(1:i)));
493         elseif(j <= i)
494           T(i,j) = delayProb(j);
495         else
496           error('ERROR');
497         end
498       end
499     end
500   end % function MakeTransitionMatrix
501   
502   %% MakeFMatrix
503   function F = MakeFMatrix(packetLength,dimU)
504 	%========================================================================
505 	% MakeFMatrix: makes the matrix F of the system which governs the
506 	% control input selection at the actuator. The matrix F maps performs one
507 	% step iteration of the vector containig possible futrue control inputs.
508   %
509   % Synopsis:
510   % [F] = MakeFMatrix(packetLength,dimU)
511   %
512   % Input Parameter:
513   % - packetLength: control sequence length; (positive integer)
514   % - dimU: dimension of the control value; (positive integer)
515   % 
516   % Output Parameter:
517   % - F: system matrix of the network-actuator subsystem; 
518   %      (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x
519   %       <dimU*packetLength*(packetLength-1)/2>)
520   %========================================================================
521     
522     if(packetLength == 1)
523       F = [];
524     else
525       F = zeros(packetLength-1,1);
526       for i = 1 : packetLength-2
527         F = [F, zeros(size(F,1),packetLength-i);...
528            zeros(packetLength-i-1,size(F,2)), eye(packetLength-i-1),...
529            zeros(packetLength-i-1,1)]; %#ok
530       end   
531     end        
532     F = kron(F,eye(dimU));
533   end % function MakeFMatrix
534   
535   %% MakeGMatrix
536   function G = MakeGMatrix(packetLength,dimU)
537   %========================================================================
538   % MakeGMatrix: matrix G maps the entries of the current control sequence
539   % which are meant to bea applied to the plant at future time steps to the
540   % vector containing possible future control inputs.
541   %
542   % Synopsis:
543   % [G] = MakeGMatrix(packetLength,dimU)
544   %
545   % Input Parameter:
546   % - packetLength: control sequence length; (positive integer)
547   % - dimU: dimension of the control value; (positive integer)
548   %
549   % Output Parameter:
550   % - G: input matrix of the network-actuator subsystem;
551   %      (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x 
552   %       <dimU*packetLength>)
553   %========================================================================
554 
555     G = zeros((packetLength*(packetLength-1)/2),packetLength);
556     G(1:(size(G,2)-1) , 2:size(G,2)) = eye(packetLength-1);
557     G = kron(G,eye(dimU));
558   end % function MakeGMatrix
559     
560   %% MakeHMatrices
561   function H = MakeHMatrices(packetLength,dimU)
562   %========================================================================
563   % MakeHMatrices: matrix H selects a control input from the vector of
564   % containing possible control inputs according to the state of the Markov
565   % chain which governs the packet arrival process at the actuator.
566   %
567   % Synopsis:
568   % [H] = MakeHMatrices(packetLength,dimU)
569   %
570   % Input Parameter:
571   % - packetLength: control sequence length; (positive integer)
572   % - dimU: dimension of the control value; (positive integer)
573   %
574   % Output Parameter:
575   % - H: 3D state measurement matrix of the network-actuator subsystem;
576   %      (matrix of dimension <dimU> x 
577   %       <dimU*packetLength*(packetLength-1)/2> x <packetLength+1>) 
578   %
579   % H(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
580   %========================================================================
581 
582     H = zeros(dimU,dimU*(packetLength*(packetLength-1)/2),packetLength+1);
583     
584     for i = 1 : packetLength-1
585       onePos = dimU*(packetLength*(packetLength-1)/2+1-...
586                (packetLength-i+1)*(packetLength-i)/2);
587       H(:,onePos:onePos+dimU-1,i+1) = eye(dimU);
588     end
589   end % function MakeHMatrices
590   
591   %% MakeJMatrices
592   function J = MakeJMatrices(packetLength,dimU)
593   %========================================================================
594   % MakeJMatrices: matrix J selects the first control input from control
595   % input sequence according to the state of the Markov chain which governs
596   % the packet arrival at the actuator.
597   %
598   % Synopsis:
599   % [J] = MakeJMatrices(packetLength,dimU)
600   %
601   % Input Parameter:
602   % - packetLength: control sequence length; (positive integer)
603   % - dimU: dimension of the control value; (positive integer)
604   %
605   % Output Parameter:
606   % - J: 3D input measurement matrix of the network-actuator subsystem;
607   %      (matrix of dimension <dimU> x <dimU*packetLength> x 
608   %       <packetLength+1>) 
609   %
610   % J(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
611   %========================================================================
612   
613     J = zeros(dimU,dimU*packetLength,packetLength+1);
614     J(:,1:dimU,1) = eye(dimU);
615   end % function MakeJMatrices
616   
617   %% MakeAugmentedStateMatrices
618   function [augA,augB] = MakeAugmentedStateMatrices(A,B,F,G,H,J,...
619                                                     packetLength)
620   %========================================================================
621   % MakeAugmentedStateMatrices: calculates the matrices of the augmented
622   % state space which describes the considered NCS as a Markov Jump Linear
623   % System.
624   %
625   % Synopsis:
626   % [augA,augB] = MakeAugmentedStateMatrices(A,B,F,G,H,J,packetLength)
627   %
628   % Input Parameter:
629   % - A: state matrix; (matrix of dimension <dimX> x <dimX>)
630   % - B: input matrix; (matrix of dimension <dimX> x <dimU>)
631   % - F: system matrix of the network-actuator subsystem; 
632   %      (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x
633   %       <dimU*packetLength*(packetLength-1)/2>)
634   % - G: input matrix of the network-actuator subsystem;
635   %      (matrix of dimension <dimU*packetLength*(packetLength-1)/2> x 
636   %       <dimU*packetLength>)
637   % - H: 3D state measurement matrix of the network-actuator subsystem;
638   %      (matrix of dimension <dimU> x 
639   %       <dimU*packetLength*(packetLength-1)/2> x <packetLength+1>) 
640   % - J: 3D input measurement matrix of the network-actuator subsystem;
641   %      (matrix of dimension <dimU> x <dimU*packetLength> x 
642   %       <packetLength+1>) 
643   % - packetLength: control sequence length; (positive integer)
644   %
645   % Output Parameter:
646   % - augA: 3D augmented state space system matrix;
647   %        (matrix of dimension <dimState> x <dimState> x <packetLength+1>)
648   % - augB: 3D augmented state space measurement matrix;
649   %         (matrix of dimension <dimState> x <dimU*packetLength> x
650   %          <packetLength+1>)
651   % 
652   % augA(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
653   % augB(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
654   %========================================================================
655     
656     [numRowsA,numColumnsA] = size([B*H(:,:,1);F]);     
657     dimA = size(A,1);    
658     augA = zeros(numRowsA, dimA+numColumnsA, packetLength+1);
659      
660     [numRowsB,numColumnsB] = size([B*J(:,:,1);G]);      
661     augB = zeros(numRowsB,numColumnsB, packetLength+1);
662     
663     for i = 1:(packetLength+1)
664         augA(1:dimA,1:dimA,i) = A;
665         augA(1:numRowsA,dimA+1:dimA+numColumnsA,i) = [B*H(:,:,i);F];
666        
667         augB(1:numRowsB,1:numColumnsB,i) = [B*J(:,:,i);G];                     
668     end
669   end % function MakeAugmentedStateMatrices
670   
671   %% MakeAugmentedCostMatrices
672   function [augQ,augR] = MakeAugmentedCostMatrices(Q,R,H,J,packetLength)
673   %========================================================================
674   % MakeAugmentedStateMatrices: expresses the cost matrices in terms of the
675   % augmented state space.
676   % 
677   % Synposis:
678   % [augQ,augR] = MakeAugmentedCostMatrices(Q,R,H,J,packetLength)
679   %
680   % Input Parameter:
681   % - Q: system state cost matrix;
682   %      (positive semidefinite matrix of dimension <dimX> x <dimX>)
683   % - R: input cost matrix;
684   %      (positive definite matrix of dimension <dimU> x <dimU>)
685   % - H: 3D state measurement matrix of the network-actuator subsystem;
686   %      (matrix of dimension <dimU> x 
687   %       <dimU*packetLength*(packetLength-1)/2> x <packetLength+1>) 
688   % - J: 3D input measurement matrix of the network-actuator subsystem;
689   %      (matrix of dimension <dimU> x <dimU*packetLength> x 
690   %       <packetLength+1>) 
691   % - packetLength: control sequence length; (positive integer)
692   % 
693   % Output Parameter:
694   % - augQ: 3D augmented state space system state cost matrix;
695   %        (matrix of dimension <dimState> x <dimState> x <packetLength+1>)
696   % - augR: 3D augmented state space control sequence cost matrix;
697   %         (matrix of dimension <dimU*packetLength> x 
698   %          <dimU*packetLength> x <packetLength+1>)
699   %
700   % augQ(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
701   % augR(:,:,markovMode), markovMode: 1...packetLength+1 (1 = no delay)
702   %========================================================================
703   
704     sizeQ = size(Q,1) + size(H(:,:,1)'*R*H(:,:,1),1);    
705     
706     augQ = zeros(sizeQ, sizeQ, packetLength+1);
707     
708     augR = zeros(size(J(:,:,1)'*R*J(:,:,1),1),...    
709                size(J(:,:,1)'*R*J(:,:,1),2),...
710                packetLength+1);
711              
712     for i = 1:(packetLength+1)
713         augQ(1:sizeQ,1:sizeQ,i) = ...
714                 [Q,...
715                  zeros(size(Q,1),size(H(:,:,i)'*R*H(:,:,i),2));...
716                  zeros(size(H(:,:,i)'*R*H(:,:,i),1),size(Q,2)),...
717                  H(:,:,i)'*R*H(:,:,i)];
718         augR(:,:,i) = J(:,:,i)'*R*J(:,:,i);
719     end
720   end % function MakeAugmentedCostMatrices
721 
722 end % methods static
723 end % classdef
  1 classdef NCS_ClassLinearSensor < handle
  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 < handle
  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, 12.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 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       else
496         s.C = C;
497         s.dimY = size(s.C,1);
498       end  % C
499     
500     % x_0
501       if (min(size(x_0)) ~= 1)
502         error(['** Input parameter <x_0> (initial system state)',...
503                ' must be a vector **'])
504       elseif (length(x_0) ~= size(A,1))
505         error(['** Dimension mismatch between input parameters ',...
506                '<x_0> (initial system state) and <A> (system matrix) **'])
507       else
508         s.x_0 = x_0;
509       end  % x_0
510     
511     % Exx
512       if (size(Exx,1) ~= size(Exx,2))
513         error(['** Input parameter <Exx> (initial state covariance)',...
514                ' must be square **'])
515       elseif (size(Exx,1) ~= size(A,1))
516         error(['** Dimension mismatch between input parameters <A> ',...
517                '(system state matrix) and <Exx> (initial state ',...
518                'covariance) **'])
519       elseif (sum(sum(Exx<0)))
520         error(['** Elements of the input parameter <Exx> (initial state',...
521                ' covariance) must be positive **'])
522       elseif (sum(sum(Exx ~= Exx')))
523         error(['** Input parameter <Exx> (initial state covariance) ',...
524                'must be symmetric **'])
525       else
526         s.Exx = Exx;
527       end  % Exx
528     
529     % maxMeasDelay
530       if (~isscalar(maxMeasDelay))
531         error(['** Input parameter <maxMeasDelay> (maximum measurement',...
532              ' delay allowed) must be a scalar **'])
533       elseif ((maxMeasDelay - floor(maxMeasDelay)) ~= 0)
534         error(['** Input parameter <maxMeasDelay> (maximum measurement',...
535              ' delay allowed) must be an integer **'])
536       elseif (maxMeasDelay < 0)
537         error(['** Input parameter <maxMeasDelay> (maximum measurement',...
538              ' delay allowed) must be positive **'])
539       else
540         s.maxMeasDelay = maxMeasDelay;
541       end  % maxMeasDelay
542     
543     % Eww
544       if (size(Eww,1) ~= size(Eww,2))
545         error(['** Input parameter <Eww> (system noise covariance) must',...
546                'be square **'])
547       elseif (size(Eww,1) ~= size(A,1))
548         error(['** Dimension mismatch between input parameters <A> ',...
549                '(system matrix) and <Eww> (system noise covariance) **'])
550       elseif (sum(sum(Eww<0)))
551         error(['** Elements of the input parameter <Eww> (system',...
552                ' noise covariance) must be positive **'])
553       elseif (sum(sum(Eww ~= Eww')))
554         error(['** Input parameter <Eww> (system noise covariance) must',...
555                'be symmetric **'])
556       else
557         s.Eww = Eww;
558       end  % Eww
559     
560     % Evv
561       if (size(Evv,1) ~= size(Evv,2))
562         error(['** Input parameter <Evv> (measurement noise covariance)',...
563                ' must be square **'])
564       elseif (size(Evv,1) ~= size(C,1))
565         error(['** Dimension mismatch between input parameters <C> ',...
566                '(measurement matrix) and <Evv> (measurement noise',...
567                'covariance) **'])
568       elseif (sum(sum(Evv < 0)))
569         error(['** Elements of the input parameter <Evv> (measurement',...
570                ' noise covariance) must be positive **'])
571       elseif (sum(sum(Evv ~= Evv')))
572         error(['** Input parameter <Evv> (measurement noise ',...
573                'covariance) must be symmetric **'])
574       else
575         s.Evv = Evv;
576       end  % Evv
577     
578     % controlPacketLength 
579     if (~isscalar(controlPacketLength))
580       error(['** Input parameter <controlPacketLength> (control sequence ',...
581              'packet length) must be a scalar **'])
582     elseif (controlPacketLength <= 0)
583       error(['** Input parameter <controlPacketLength> (control sequence ',...
584              'packet length) must be positive **'])
585     else
586       s.controlPacketLength = controlPacketLength;
587     end % controlPacketLength
588     
589     % controlDelayProb
590     if (min(size(controlDelayProb)) ~= 1)
591       error(['** Input parameter <controlDelayProb> (packet delay ',...
592              'probability distribution) must be a vector **'])
593     elseif (numel(controlDelayProb) < s.controlPacketLength)
594       error(['** Input parameter <controlDelayProb> (packet delay ',...
595              'probability distribution) is too short **'])
596     elseif (abs(sum(controlDelayProb)-1) > 1e-6)
597       error(['** Sum of elements in the input parameter <controlDelayProb>',...
598              ' (packet delay probability distribution) must be 1 **'])
599     else
600       s.controlDelayProb = controlDelayProb;
601       if (numel(s.controlDelayProb) == s.controlPacketLength)
602         s.controlDelayProb(end+1) = 0;
603       end
604     end % controlDelayProb
605      
606   end % function ProcessInputs
607 end % methods private
608 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
609 methods (Static)
610   %% CalcDelayTrans
611   function [ T ] = CalcDelayTrans( uDelProbs )
612   %========================================================================
613   % CalcDelayTrans: calculates time delay transition matrix
614   % The function determines the transition matrix a the Markov chain whose
615   % state describes the time delay of the combined model of communication
616   % network and actuator.
617   %
618   % Synopsis:
619   % [trans] = CalcDelayTrans( uDelProbs )
620   % 
621   % Input Prameter:
622   % - uDelProbs: describes the probabilities of control input time delays.
623   %   Thereby the i-th entry of delayProb is the probability that a delay 
624   %   of i - 1 time steps occurs. Sum of vector must be equal to 1;
625   %   E.g.: uDelProbls = [0.1 0.9] means that a packet will pass the 
626   %   network without delay with probability 0.1. A delay of one time step
627   %   occurs with probability 0.9.
628   %   (vector of positive floats)
629   %
630   % Output Paramter:
631   % - trans: transition matrix of the Markov chain
632   %   (matrix with dimension numel(uDelProbs) x numel(uDelProbs))
633   %
634   % See also: ClassCommNetwork, NCS_ClassDelayedKF, ClassIMM
635   %========================================================================
636   
637   % check input paramter
638   if( size(uDelProbs, 1) ~= 1 )
639     uDelProbs %#ok<NOPRT>
640     error('** Input parameter uDelProbs is not a row vector **')
641   elseif( min(uDelProbs) < 0 )
642       uDelProbs %#ok<NOPRT>
643       error('** Input parameter uDelProbs has negative entries **')
644   elseif( (round(sum(uDelProbs .* 1000000)) / 1000000) ~= 1 )
645     round(sum(uDelProbs .* 1000000)) / 1000000
646     uDelProbs %#ok<NOPRT>
647     error(['** Sum of entries of input paramter uDelProbs is' ...
648       ' not equal to 1 **'])
649   end
650   % determine number of states of the Markov chain
651   numStates = numel(uDelProbs);
652 
653     T = zeros(numStates,numStates);
654 
655     % calculate Transition matrix
656     for i = 1 : numStates
657       for j = 1 : numStates
658         if j >= i +2
659           T(i,j) = 0;
660         elseif j== i + 1
661           T(i,j) = 1 - (sum(uDelProbs(1:i)));
662         elseif j <= i
663           T(i,j) = uDelProbs(j);
664         else
665           error('ERROR');
666         end
667       end
668     end
669   end % function CalcDelayTrans
670 end % methods static
671 end % classdef
 1 classdef NCS_ClassDataPacket < handle
 2 %==========================================================================
 3 % NCS_ClassDataPacket: defines a data packet used to transmit data over a
 4 % communication network described by the class ClassCommNetwork.
 5 %
 6 % NCS_ClassDataPacket()
 7 % NCS_ClassDataPacket( data , timeStamp , packetDelay )
 8 %
 9 % AUTHOR: Jörg Fischer
10 % LAST UPDATE: Maxim Dolgov, 12.04.2013
11 %==========================================================================
12 
13 properties (Access = public)
14   %% input parameters
15   % time Stamp that indicates time step when data packet was generated;
16   % (positive integer or NaN)
17   timeStamp = -1;
18   % value of data packet. Can be any data; (application dependend data 
19   % type) 
20   value = NaN;
21   % time delay of the data packet, measured in time steps; 
22   % (positive integer or NaN)
23   packetDelay = -1;
24 end % properties
25 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
26   methods
27   %% NCS_ClassDataPacket
28   function s = NCS_ClassDataPacket(varargin)
29   %========================================================================
30   % NCS_ClassDataPacket: creates a handler to an object of type 
31   % (NCS_ClassDataPacket)
32   % 
33   % Synopsis:
34   % [s] = NCS_ClassDataPacket(value, timeStamp)
35   % [s] = NCS_ClassDataPacket()
36   %
37   % Input Parameter:
38   % - value: data payload carried by the data packet
39   % - timeStamp: system type indicating when the packet was sent
40   %
41   % Output Parameter:
42   % - s: handler to the created data packet
43   %========================================================================
44 
45     if nargin >= 1
46       if(nargin == 1)
47         s.value = varargin{1};
48       elseif nargin == 2
49         s.value = varargin{1};
50         s.timeStamp = varargin{2};
51       else
52         error('** Wrong number of input arguments **')
53       end
54     elseif(nargin == 0)
55       % take default values
56     end
57   end % function NCS_ClassDataPacket
58     
59   %% SetPacketDelay
60   function SetPacketDelay(s, packetDelay)
61 	%========================================================================
62   % SetpacketDelay: sets time delay experiences by the packet
63   % 
64   % Synopsis:
65   % SetpacketDelay(packetDelay)
66   %
67   % Input Parameter:
68   % - packetDelay: time delay experienced by the data packet
69   %
70   % Output Parameter:
71   % -- none --
72   %========================================================================
73 
74     if ~isscalar(packetDelay)
75       error(['** Input parameter <packetDelay> (time delay experienced ',...
76              'by the data packet) is not a scalar **'])
77     end
78     if (packetDelay < 0) || ((packetDelay - floor(packetDelay)) ~= 0)
79       error(['** Input parameter <packetDelay> (time delay experienced ',...
80              'by the data packet) must be a positive integer or zero **'])
81     end
82       s.packetDelay = packetDelay;
83   end % function SetPacketDelay
84 end % methods public
85 end % classdef
  1 classdef NCS_ClassCommNetwork < handle
  2 %==========================================================================
  3 % NCS_ClassCommNetwork: This class simulates a communication network to send
  4 % and receive data packets of type NCS_ClassDataPacket. A data packet that 
  5 % enters the network will be subject to a stochastic time delays. If the 
  6 % 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 %
 16 % AUTHOR: Jörg Fischer
 17 % LAST UPDATE: Maxim Dolgov, 30.04.2013
 18 %==========================================================================
 19   
 20 properties (SetAccess = private)
 21   %% input parameters
 22     % length of simulation time in time steps; (positive integer)
 23     simTime = -1;
 24     % maximal time delay;
 25     % The network does not output a data packet that has a longer time 
 26     % delay; (positive integer)
 27     maxDelay = -1;
 28   %% derived parameters
 29     % cell array with data packets to store input data;
 30     % (array of NCS_ClassDataPacket)
 31     nwData = {};
 32     % number of packets that leave the network to current time step;
 33     % Index is the time step; (array of positive integer)
 34     numPacketsOut = [];
 35     % current time step; (positive integer)
 36     k = -1;
 37     % debug variable
 38     debugInfo = {};
 39 
 40     % tolerance interval for rounding error
 41     roundingErr = 0.00000001;
 42 end % properties
 43 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
 44 methods
 45   %% NCS_ClassCommNetwork
 46   function s = NCS_ClassCommNetwork(simTime,maxDelay)
 47   %========================================================================
 48   % NCS_ClassCommNetwork: Constructs object of a communication network
 49   %
 50   % Synopsis:
 51   % [s] = NCS_ClassCommNetwork(simTime,maxDelay)
 52   %
 53   % Input Paramter:
 54   % - Param: structure containing class parameters
 55   % - simTime: time steps of simulation; (positive integer)
 56   % - maxDelay: maximal time delay possible. The value must be equal to
 57   %             or smaller than the number of entries in the vector
 58   %             <delayProb>, if it is defined, or to the maximum delay
 59   %             in the vector packetDelays, if this one is defined;
 60   %             (positive integer)
 61   % 
 62   % Output Paramter:
 63   % - s: handler to the created object; (NCS_ClassCommNetwork)
 64   %
 65   %========================================================================
 66   if (nargin>=1)
 67     % ------- 0) check input parameters and init data:
 68     s.ProcessInputs(simTime,maxDelay)
 69 
 70     % init network data
 71     s.numPacketsOut = zeros(simTime, 1);
 72     s.nwData = {};
 73     % To keep track of packets that will leave the network to the same
 74     % time step the network data array is two-dimensional
 75     s.nwData{simTime, 1} = NCS_ClassDataPacket();
 76     
 77     % set system time
 78     s.k = 1;
 79   end
 80   end % function NCS_ClassCommNetwork
 81   
 82   %% DataInDataOut
 83   function [numPackets, packetsOut] = DataInDataOut(s, packet, packetDelay)
 84   %========================================================================
 85   % DataInDataOut: Sending and receiving data over the network.
 86   % connection. Every function call 1) a new data packet can be sent
 87   % over the network (optional) and 2) a data packet leaves the network.
 88   %
 89   % Synopsis: 
 90   % [numPackets, packetsOut] = DataInDataOut(packet, packetDelay)
 91   % [numPackets, packetsOut] = DataInDataOut([], [])
 92   %
 93   % Input Paramter:
 94   % - packet: data packet sent into the network; (NCS_ClassDataPacket) 
 95   % - packetDelay: time delay which will be experienced by the packet;
 96   %                (positive integer)
 97   % - []: empty parameter, if no packet shall be sent
 98   %
 99   % Output Paramter:
100   % - numPackets: number of packets that leave the network; (integer) 
101   % - packetsOut: datapacket/s that leave the network;
102   %   (array of NCS_ClassDataPacket)
103   %
104   % REMARK: this function must to be executed once every time step
105   %========================================================================
106   
107     % check input parameters
108     if(isempty(packet))
109       packetReceived = false;
110     else
111       % check packet
112       if(~isa(packet, 'NCS_ClassDataPacket'))
113         error(['** Input parameter <packet> (sent data packet) is not ',...
114                'of class NCS_ClassDataPacket **'])
115       end % check packet
116       
117       % check packetDelay
118       if(~isscalar(packetDelay))
119         error(['** Input parameter <packetDelay> (time delay to ',...
120                ' experience by the packet) must be a scalar **'])
121       elseif(packetDelay < 0)
122         error(['** Input parameter <packetDelay> (time delay to ',...
123                ' experience by the packet) must be positive or zero **'])
124       end % check packetDelay
125       
126       packetReceived = true;
127       
128     end % check input parameters
129     
130     % put the received packet in queue if any
131     if(packetDelay > s.maxDelay) ...
132           || (packetReceived == false)  ...
133           || ((s.k + packetDelay) > s.simTime)
134       % packet lost or no new packet available or packet would
135       % leave network after end of simulation
136       % -> do not take packet in data array
137       % debug Info
138       s.debugInfo{1, s.k} = s.k;
139       s.debugInfo{2, s.k} = NaN;
140       s.debugInfo{3, s.k} = packetDelay;
141     else
142       % warn if packet time stamp does not correspond to transmission time
143       if(packet.timeStamp ~= s.k)
144         disp(['Warning: packet time stamp does not correspond to ',...
145               'transmission time'])
146       end
147       
148       % new packet sent into network:
149       % a) update packet info
150       packet.SetPacketDelay(packetDelay);
151       % b) increment packet output counter for the time step when this 
152       % packet will leave
153       i = s.numPacketsOut(s.k + packetDelay) + 1;
154       s.numPacketsOut(s.k + packetDelay) = i;
155       % c) write data in network data cell array
156       s.nwData{s.k + packetDelay, i} = packet;
157       % Storing data for debug
158       s.debugInfo{1, s.k} = s.k;
159       s.debugInfo{2, s.k} = packet.value;
160       s.debugInfo{3, s.k} = packetDelay;
161     end % put the received packet in queue
162     
163     % network output
164     % first output parameter is number of packets that will leave the
165     % network
166     numPackets = s.numPacketsOut(s.k);
167     % the second output parameter is(are) the packet(s) that will leave
168     % the network to current time step
169     if( numPackets == 0 )
170       % no packet
171       packetsOut = NCS_ClassDataPacket();
172       % storing data for debug
173       s.debugInfo{5, s.k} = packetsOut.value;
174       s.debugInfo{6, s.k} = packetsOut.timeStamp;
175       s.debugInfo{7 , s.k} = packetsOut.packetDelay;
176     else
177       % allocate memory for output packets
178       packetsOut(numPackets) = NCS_ClassDataPacket();
179       for i = 1:numPackets
180         packetsOut(i) = s.nwData{s.k, i};
181         % storing data for debug
182         s.debugInfo{2 + i * 3, s.k} = packetsOut(i).value;
183         s.debugInfo{3 + i * 3, s.k} = packetsOut(i).timeStamp;
184         s.debugInfo{4 + i * 3, s.k} = packetsOut(i).packetDelay;
185       end
186     end
187     s.debugInfo{4, s.k} = '------------';
188 
189     % increment time step
190     s.k = s.k + 1;
191   end % function DataInDataOut
192   
193   %% Reset
194   function Reset(s)
195   %========================================================================
196   % Reset: puts the sensor in the initial condition
197   %
198   % Synopsis:
199   % Reset()
200   %
201   % Input Parameter:
202   % -- none --
203   % 
204   % Output Parameter:
205   % -- none --
206   %========================================================================
207     % init network data
208     s.numPacketsOut = zeros(s.simTime, 1);
209     s.nwData = {};
210     % To keep track of packets that will leave the network to the same
211     % time step the network data array is two-dimensional
212     s.nwData{s.simTime, 1} = NCS_ClassDataPacket();
213     s.k = 1;
214   end % function Reset
215 end % methods public
216 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
217 methods (Access = private)
218   %% ProcessInputs
219   function ProcessInputs(s,simTime,maxDelay)
220 	%========================================================================
221   % ProcessInputs: check and store provided class inputs
222   %
223   % 
224   %========================================================================
225 
226     % simTime
227       if (~isscalar(simTime))
228         error(['** Input parameter <simTime> (simulation time) is ',...
229                'not a scalar**'])
230       elseif (simTime < 0)
231         error(['** Input parameter <simTime> (simulation time) must',...
232                ' be positive **'])
233       elseif ((simTime - floor(simTime)) ~= 0)
234         error(['** Input parameter <simTime> (simulation time) must be',...
235                ' an integer **'])
236       else 
237         s.simTime = simTime;
238       end
239     % check simTime
240 
241     % maxDelay
242       if ~isscalar(maxDelay)
243         error(['** Input parameter <maxDelay> (maximum possible packet',...
244                ' delay) is not a scalar **'])
245       elseif((maxDelay < 0) || ...
246              ((maxDelay - floor(maxDelay)) ~= 0))
247         error(['** Input parameter <maxDelay> (maximum possible packet',...
248                ' delay) is not a positive integer **'])
249       else
250         s.maxDelay = maxDelay;
251       end
252    % check maxDelay   
253 
254   end % function ProcessInputs 
255 end % methods private
256 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
257 methods (Static)
258   %% DiscreteSample
259   function x = DiscreteSample(p, n)
260   %========================================================================
261   % DiscreteSample: Samples from a discrete distribution: independently 
262   % draws n samples (with replacement) from the distribution specified by 
263   % p, where p is a probability array whose elements sum to 1.
264   %
265   % Synopsis:
266   % [x] = DiscreteSample(p, n)
267   % 
268   % Input Parameter:
269   % - p: probability density function; (vector of dimension K)
270   % - n: number of drawn samples; (positive integer)
271   %
272   % Output Parameter:
273   % - x: samples from a discrete distribution
274   %
275   % REMARK: Created by Dahua Lin, On Oct 27, 2008
276   %========================================================================
277 
278     % parse and verify input arguments
279     assert(isfloat(p), 'discretesample:invalidarg', ...
280         'p should be an array with floating-point value type.');
281 
282     assert(isnumeric(n) && isscalar(n) && n >= 0 && n == fix(n), ...
283         'discretesample:invalidarg', ...
284         'n should be a nonnegative integer scalar.');
285 
286     % main
287     % process p if necessary
288     K = numel(p);
289     if ~isequal(size(p), [1, K])
290         p = reshape(p, [1, K]);
291     end
292 
293     % construct the bins
294     edges = [0, cumsum(p)];
295     s = edges(end);
296     if abs(s - 1) > eps
297         edges = edges * (1 / s);
298     end
299 
300     % draw bins
301     rv = rand(1, n);
302     c = histc(rv, edges);
303     ce = c(end);
304     c = c(1:end-1);
305     c(end) = c(end) + ce;
306 
307     % extract samples
308     xv = find(c);
309 
310     if numel(xv) == n  % each value is sampled at most once
311         x = xv;
312     else                % some values are sampled more than once
313         xc = c(xv);
314         d = zeros(1, n);
315         dv = [xv(1), diff(xv)];
316         dp = [1, 1 + cumsum(xc(1:end-1))];
317         d(dp) = dv;
318         x = cumsum(d);
319     end
320 
321     % randomly permute the sample's order
322     x = x(randperm(n));
323   end % function DiscreteSample
324 end % methods static
325 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
  1 function main(simNetwork,simTime,numSimRuns,disturbtion,examplePacketLength)
  2 % This function performs a Monte Carlo simulation of the optimal
  3 % sequence-based LQG controller proposed in J. Fischer, A. Hekler, M.
  4 % Dolgov, and U. Hanebeck "Optimal Sequence-Based LQG Control over TCP-like 
  5 % Networks Subject to Random Transmission Delays and Packet Losses"
  6 % <a href="matlab:web('http://arxiv.org/abs/1211.3020')">[link]</a>.
  7 %
  8 % In the simulated scenario the controller drives a disturbed double
  9 % integrator to the origin and keeps it there.
 10 %
 11 % The output consits of two figures. Figure 1 shows the properties of the
 12 % simulated network and the mean cumulated costs per time step. Figure 2
 13 % shows an example trajectory with a realization of the process which
 14 % generates the delays in the controller-actuator channel.
 15 %
 16 % Input Parameters:
 17 % - simNetwork: ID of the simulated network;
 18 %               (ID = 1: "good" network, ID = 2: "bad" network)
 19 % - simTime: number of time steps per one Monte Carlo simulation run
 20 % - numSimRuns: number of Monte Carlo simulation runs
 21 %               (the higher, the better)
 22 % - disturbtion: mean value of the disturbtion
 23 % - examplePacketLength: packet length for the example trajectory
 24 %
 25 
 26   % generate random generator seed from current system time
 27   c = clock;
 28   rng(sum(c(4:6).*06.^(1:3)))
 29   
 30   % load simulation networks
 31     NetworkDelayDistribution = load('SimulatedNetworks.mat');
 32     NetworkDelayDistribution = ...
 33                            NetworkDelayDistribution.NetworkDelayDistribution;
 34                          
 35     maxPacketLength = numel(NetworkDelayDistribution(1,:))-1;
 36     scDelayProb = [1 0 0 0];
 37     scMaxDelay = 10;
 38 
 39   % system parameters
 40     % System Parameters:
 41     A = [1 1;0 1];
 42     B = [0;1];
 43     C = [1 0];
 44     x0Cov = [.5^2 0;0 .5^2];
 45     wCov = [.1^2 0;0 .1^2];
 46     vCov = .2^2;
 47     Q = eye(2);
 48     R = 1;
 49     
 50     x0Mean = [disturbtion;0];
 51     
 52     caDelayProb = NetworkDelayDistribution(simNetwork,:);
 53   
 54 	% container for costs
 55   J = zeros(1,10);
 56   
 57 	% perform Monte Carlo Simulation
 58 for caPacketLength = 1 : maxPacketLength
 59   % create the control loop
 60   ncs = NCS_ClassTcpNcs(A,B,C,Q,R,x0Mean,x0Cov,wCov,vCov,simTime,caDelayProb,caPacketLength,scMaxDelay);
 61   tmp = zeros(1,numSimRuns);
 62 
 63   for k = 1 : numSimRuns
 64   % initialize stochastic parameters
 65     % create system and measurement noise
 66     w = mvnrnd(zeros(size(wCov,1),1),wCov,simTime)';
 67     v = mvnrnd(zeros(size(vCov,1),1),vCov,simTime)';
 68     % create initial state
 69     x_0 = x0Mean + mvnrnd(zeros(numel(x0Mean),1),x0Cov,1)';
 70     % create packet delays
 71     caPacketDelays = DiscreteSample(caDelayProb,simTime)-1;
 72     scPacketDelays = DiscreteSample(scDelayProb,simTime)-1;
 73   % simulate the control loop
 74     results = ncs.Simulate(x_0,w,v,caPacketDelays,scPacketDelays);
 75     tmp(k) = results.costs;
 76   end % Monte Carlo simulation for specific packet length
 77   % store simulation results
 78   J(caPacketLength) = mean(tmp);
 79 end % loop through all packet length
 80 
 81   % compute example trajectory
 82   caPacketLength = examplePacketLength;
 83   w = mvnrnd(zeros(size(wCov,1),1),wCov,simTime)';
 84   v = mvnrnd(zeros(size(vCov,1),1),vCov,simTime)';
 85   x_0 = x0Mean + mvnrnd(zeros(numel(x0Mean),1),x0Cov,1)';
 86   caPacketDelays = DiscreteSample(caDelayProb,simTime)-1;
 87   scPacketDelays = DiscreteSample(scDelayProb,simTime)-1;
 88 
 89   ncs = NCS_ClassTcpNcs(A,B,C,Q,R,x0Mean,x0Cov,wCov,vCov,simTime,caDelayProb,caPacketLength,scMaxDelay);
 90   results = ncs.Simulate(x_0,w,v,caPacketDelays,scPacketDelays);
 91   
 92   % display Monte Carlo results
 93   clf(figure(1))
 94   figure(1)
 95   subplot(2,1,1)
 96   bar(0:13,[NetworkDelayDistribution(simNetwork,1:10), 0 0 0, NetworkDelayDistribution(simNetwork,11)])
 97   xlabel('Packet Delay in Time Steps')
 98   ylabel('Probability')
 99   axis([-.5 13.5 0 .8])
100   set(gca,'XTickLabel',{'0','1','2','3','4','5','6','7','8','9','','','','Inf'})
101   subplot(2,1,2)
102   plot(1:maxPacketLength,J)
103   xlabel('Packet Length')
104   ylabel('Mean Cumulated Costs per Time Step')
105   drawnow
106   
107   % display example trajectory
108   clf(figure(2))
109   figure(2)
110   subplot(3,1,1)
111   plot([0, 1:simTime],results.realTrajectory(1,:))
112   hold on
113   plot([0, 1:simTime],results.estimatedTrajectory(1,:),'g')
114   plot([0, 1:simTime],zeros(1,simTime+1),'r')
115   xlabel('Time Step')
116   ylabel('x_1')
117   legend('Real Trajectory', 'Estimated Trajectory')
118   subplot(3,1,2)
119   plot([0, 1:simTime],results.realTrajectory(2,:))
120   hold on
121   plot([0, 1:simTime],results.estimatedTrajectory(2,:),'g')
122   plot([0, 1:simTime],zeros(1,simTime+1),'r')
123   legend('Real Trajectory', 'Estimated Trajectory')
124   xlabel('Time Step')
125   ylabel('x_2')
126   subplot(3,1,3)
127   bar(1:simTime,caPacketDelays)
128   xlabel('Time Step')
129   ylabel('Delays of Sent Packet')
130   drawnow
131 end % function main
  1 function x = DiscreteSample(p, n)
  2   % DiscreteSample: Samples from a discrete distribution
  3   %
  4   %   x = discretesample(p, n)
  5   %       independently draws n samples (with replacement) from the 
  6   %       distribution specified by p, where p is a probability array 
  7   %       whose elements sum to 1.
  8   %
  9   %       Suppose the sample space comprises K distinct objects, then
 10   %       p should be an array with K elements. In the output, x(i) = k
 11   %       means that the k-th object is drawn at the i-th trial.
 12   %       
 13   %   Remarks
 14   %   -------
 15   %       - This function is mainly for efficient sampling in non-uniform 
 16   %         distribution, which can be either parametric or non-parametric.         
 17   %
 18   %       - The function is implemented based on histc, which has been 
 19   %         highly optimized by mathworks. The basic idea is to divide
 20   %         the range [0, 1] into K bins, with the length of each bin 
 21   %         proportional to the probability mass. And then, n values are
 22   %         drawn from a uniform distribution in [0, 1], and the bins that
 23   %         these values fall into are picked as results.
 24   %
 25   %       - This function can also be employed for continuous distribution
 26   %         in 1D/2D dimensional space, where the distribution can be
 27   %         effectively discretized.
 28   %
 29   %       - This function can also be useful for sampling from distributions
 30   %         which can be considered as weighted sum of "modes". 
 31   %         In this type of applications, you can first randomly choose 
 32   %         a mode, and then sample from that mode. The process of choosing
 33   %         a mode according to the weights can be accomplished with this
 34   %         function.
 35   %
 36   %   Examples
 37   %   --------
 38   %       % sample from a uniform distribution for K objects.
 39   %       p = ones(1, K) / K;
 40   %       x = discretesample(p, n);
 41   %
 42   %       % sample from a non-uniform distribution given by user
 43   %       x = discretesample([0.6 0.3 0.1], n);
 44   %
 45   %       % sample from a parametric discrete distribution with
 46   %       % probability mass function given by f.
 47   %       p = f(1:K);
 48   %       x = discretesample(p, n);
 49   %
 50 
 51   %   Created by Dahua Lin, On Oct 27, 2008
 52   %
 53 
 54     % parse and verify input arguments
 55 
 56     assert(isfloat(p), 'discretesample:invalidarg', ...
 57         'p should be an array with floating-point value type.');
 58 
 59     assert(isnumeric(n) && isscalar(n) && n >= 0 && n == fix(n), ...
 60         'discretesample:invalidarg', ...
 61         'n should be a nonnegative integer scalar.');
 62 
 63     % main
 64 
 65     % process p if necessary
 66 
 67     K = numel(p);
 68     if ~isequal(size(p), [1, K])
 69         p = reshape(p, [1, K]);
 70     end
 71 
 72     % construct the bins
 73 
 74     edges = [0, cumsum(p)];
 75     s = edges(end);
 76     if abs(s - 1) > eps
 77         edges = edges * (1 / s);
 78     end
 79 
 80     % draw bins
 81 
 82     rv = rand(1, n);
 83     c = histc(rv, edges);
 84     ce = c(end);
 85     c = c(1:end-1);
 86     c(end) = c(end) + ce;
 87 
 88     % extract samples
 89 
 90     xv = find(c);
 91 
 92     if numel(xv) == n  % each value is sampled at most once
 93         x = xv;
 94     else                % some values are sampled more than once
 95         xc = c(xv);
 96         d = zeros(1, n);
 97         dv = [xv(1), diff(xv)];
 98         dp = [1, 1 + cumsum(xc(1:end-1))];
 99         d(dp) = dv;
100         x = cumsum(d);
101     end
102 
103     % randomly permute the sample's order
104     x = x(randperm(n));
105   end % function DiscreteSample
Download algorithm (11 files) as ZIP

Comments

Please login to post a comment.