CloudRunner

... share your algorithms as a web service

Algorithm: Interacting Multiple Model (IMM) filter for Networked Control Systems (NCS)

Description:
The files provide an estimator for a sequence-based networked control system (NCS) where the control inputs and measurements are transmitted via a network that is subject to stochastic time delays and stochastic data losses. The estimator is non-linear and based on the interacting multiple model (IMM) filter algorithm. The algorithm ... (show more)
Paper: Jörg Fischer, Achim Hekler, Uwe D. Hanebeck,
State Estimation in Packet-Based Networked Control Systems,
Proceedings of the 15th International Conference on Information Fusion (Fusion 2012) (to appear), Singapore, July, 2012.
Tags: sequencebased systems control networked IMM delay time loss NCS
Usage: Algorithm is public.
Viewed 3935 times, called 174 times
Upload:
Full star Full star Full star Full star Full star
1 vote
Jörg Fischer
07/31/2012 10:11 a.m. (version #2)

Select Version

Version #2: 07/31/2012 10:11 a.m.
No changelog available

Version #1: 07/05/2012 4:23 p.m.
No changelog available

Run Algorithm

: Number of total Monte Carlo simulation runs. One run consists of <simCycles> number of time steps, where the length of one time step is defined by the sample time. The simulation
parameters are given as data sets and can change betweed two simulation run. After all simulation runs the RMSE is build by averaging all RMSE of a specific time step for each filter
<positive integer>
: Number of time steps simulated per simulation run
<positive integer>
: Number of time steps after which the setpoint of the pendulum is reversed, e.g., from [3, 0] to [-3, 0]
<positive integer>
: Time delay probability distribution of the CONTROLLER-ACTUATOR-NETWORK. Thereby the i-th entry of this vector is the probability that a delay of i time steps occurs; the last entry of this vector gives the data loss rate of the of the network (if vector has three or more entries)
Sum of this vector must be equal to 1
<vector of positive floats>
: Time delay probability distribution of the SENSOR-ESTIMATOR-NETWORK. Thereby the i-th entry of this vector is the probability that a delay of i time steps occurs; the last entry of this vector gives the data loss rate of the of the network (if vector has three or more entries)
Sum of this vector must be equal to 1
<vector of positive floats>
: 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/60/interacting-multiple-model-imm-filter-for-networked-control-systems-ncs/version/2/')

    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:

  1 function x = discretesample(p, n)
  2 % 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));
 1 function keyComm = pauseButton(bHandle,bSize,posx,posy)
 2 %  function pauseButton(handle,size,xPosition,yPosition)
 3 %  default arguments:    121,  192, 50,       50
 4 %
 5 %  call frequently (say.., more than every 300ms?)
 6 %
 7 %  (c) Murphy O'Brien. 2004-2006
 8 %   all rights unreserved
 9 %
10 % Displays a button which, if clicked, causes MATLAB to pause and ask for an expression to be
11 % entered. This expression is evaluated in the calling functions workspace, the result is displayed
12 % and then MATLAB continues running its script/simulation. 
13 %
14 %pauseButton should be called often from inside a loop in a simulation which the user may want to
15 %pause. After the first call, it takes almost zero processing time.
16 %
17 %Useful if e.g. you are running a long simulation and suddenly decide you want to know how many five
18 %card poker hands are in a pack.
19 %
20 %Click the pause button, enter nchoosek(52,5) at the command window prompt then press return. 
21 % Matlab displays 
22 % ans =
23 %     2598960
24 %and continues the simulation.
25 %
26 %example usage:
27 %
28 %for ii=1:50000
29 %   pauseButton;
30 %   for jj=1:1000
31 %     dummy=randn(100)*randn(100);
32 %   end
33 %end
34 
35 if (nargin<1)
36     bHandle=121;
37 end
38 if ishandle(bHandle)
39     handles=findall(bHandle,'userData','pauseButton');
40 else
41     handles=[];
42 end
43 if any(handles==bHandle)
44         firstTime=0;
45 else
46         firstTime=1;
47 end
48 
49 % 
50 if firstTime
51     if nargin<4
52         posx=50;posy=50;
53     end
54     if (nargin<2) || (bSize<4)
55         bSize=192;
56     end
57     figure(bHandle);
58     clf;
59     set(bHandle,'userData','pauseButton')
60     set(bHandle,'position',[posx,posy,bSize,bSize]);
61     set(bHandle,'NumberTitle','off')
62     set(gcf,'Name','Pause Button')
63     edgeSize=10;
64     z=zeros(bSize);m=1+z;
65     m(:,1:edgeSize)=0.6;m(1:edgeSize,:)=0.6;                  % shaded edges
66     m(end-edgeSize+1:end,:)=0.4;m(:,end-edgeSize+1:end)=0.4;  % shaded edges
67     m=cat(3,m,m,z);                                           % a yellow button
68     image(m);
69     text(bSize/2-54,bSize/2,['Click here';' to stop  '],'FontSize',16)
70     set(bHandle,'SelectionType','extend')                     % start with extend
71     axis off
72     set(bHandle,'HandleVisibility','off')
73     drawnow;
74 end
75 drawnow    
76 try
77     st=get(bHandle,'SelectionType');
78 catch
79     return
80 end
81 if st(1)=='e'
82   % return if the button hasn't been clicked  
83   keyComm='';
84   return                                                    
85 end
86 set(bHandle,'SelectionType','extend')
87 pause(0.1)
88 % otherwise wait for a command
89 keyComm='stopIt';
90 %keyComm=input('Type an expression to evaluate or press ENTER to continue.\n','s');      
91 % and execute it, and display the error if there is one
92 %evalin('caller',estring,'disp(lasterr)')                      
 1 function [ trans ] = calcDelayTrans( uDelProbs, varargin )
 2   % calcDelayTrans: calculates time delay transition matrix
 3   % The function determines the transition matrix a the Markov chain whose
 4   % state describes the time delay of the combined model of communication
 5   % network and actuator.
 6   %
 7   % Synopsis:
 8   % [trans] = calcDelayTrans( uDelProbs )
 9   % [trans] = calcDelayTrans( uDelProbs, noZeroEntries )
10   % 
11   % Input Prameter:
12   % - uDelProbs: describes the probabilities of control input time delays.
13   %   Thereby the i-th entry of delayProb is the probability that a delay 
14   %   of i - 1 time steps occurs. Sum of vector must be equal to 1;
15   %   E.g.: uDelProbls = [0.1 0.9] means that a packet will pass the 
16   %   network without delay with probability 0.1. A delay of one time step
17   %   occurs with probability 0.9.
18   %   (vector of positive floats)
19   % - noZeroEntries: if set to 'true' zero entries of uDelProbs are 
20   %   replaced by a value almost zero but not equal to zero. This results 
21   %   in a transition matrix with all entries below the upper diagonal 
22   %   are non-zero which prevents singularities in some filter algorithms;
23   %   optional parameter; 
24   %   if omitted the value is set to true;
25   %   (logical)
26   %
27   % Output Paramter:
28   % - trans: transition matrix of the Markov chain
29   %   (matrix with dimension numel(uDelProbs) x numel(uDelProbs))
30   %
31   % See also: ClassCommNetwork, ClassDelayedKF, ClassIMM
32 
33   % ---- 0) Defining Constatns:
34   % If noZeroEntries is set to true, the entries of uDelProbs smaller than
35   % minProbVal will be set to minProbVal
36   minProbVal = 0.000000000001;
37   
38   % ---- 1) check input paramter
39   % noZeroEntries
40   if nargin > 2
41     error('** Too many input parameters **')
42   elseif nargin == 1
43     % default value: true
44     noZeroEntries = true;
45   else
46     noZeroEntries = varargin{1};
47     if ~islogical(noZeroEntries)
48       error('** Input parameter noZeroEntries is not a boolean **')
49     end
50   end
51   
52   % uDelProbs
53   if( size(uDelProbs, 1) ~= 1 )
54     uDelProbs %#ok<NOPRT>
55     error('** Input parameter uDelProbs is not a row vector **')
56   elseif( min(uDelProbs) < 0 )
57       uDelProbs %#ok<NOPRT>
58       error('** Input parameter uDelProbs has negative entries **')
59   elseif( (round(sum(uDelProbs .* 1000000)) / 1000000) ~= 1 )
60     round(sum(uDelProbs .* 1000000)) / 1000000 %#ok<NOPRT>
61     uDelProbs %#ok<NOPRT>
62     error(['** Sum of entries of input paramter uDelProbs is' ...
63       ' not equal to 1 **'])
64   elseif( noZeroEntries == true )
65     % Replace zeros in uDelProbs by minProbVal and renorm vector
66     reNorm = false;
67     for i = 1 : numel(uDelProbs)
68       if( uDelProbs(i) ) < minProbVal
69         uDelProbs(i) = minProbVal;
70         reNorm = true;
71       end
72     end
73     if reNorm == true
74       uDelProbs = uDelProbs / sum(uDelProbs);
75     end
76   end
77 
78   % ---- 2) Algorithm
79   % determine number of states of the Markov chain
80   numStates = numel(uDelProbs);
81   % preAllocation of memory
82   trans = zeros(numStates, numStates);
83   % algorithm
84   for rowCounter = 1 : numStates
85     for colCounter = 1 : numStates
86       if colCounter >= rowCounter + 2
87         % upper right triangle entries
88         trans(rowCounter, colCounter) = 0;
89       elseif colCounter == rowCounter + 1
90         % upper diagonal entries
91         trans(rowCounter, colCounter) = ...
92           1 - sum(uDelProbs(1:rowCounter));
93       else % colCounter <= rowCounter
94         % entries under upper diagnaol
95         trans(rowCounter, colCounter) = uDelProbs(colCounter);
96       end
97     end
98   end
99 end
  1 classdef ClassActuator < handle
  2   % ClassActuator: This class simulates an actuator in a packet-based NCS
  3   % configuration. The input of the actuator are data packets that
  4   % consist of time-stamped sequences of control inputs. Output is a 
  5   % single control input that is applied to the plant. The actuator
  6   % is equipped with a buffer where the control input sequence with the
  7   % most recent time stamp is stored. The actuator applies the control 
  8   % input of that buffered sequence that corresponds to the current time 
  9   % step. If there is no control input that corresponds to the current 
 10   % time step, a configurable default value is applied.
 11   %
 12   % CONDITION: the function DataInDataOut has to be executed ONCE every
 13   % time step.
 14   %
 15   % See also ClassCommNetwork, ClassDataPacket
 16 
 17   
 18   properties (SetAccess = private)
 19     % buffer to store active control input sequence; (ClassDataPacket)
 20     bufferedPacket = [];
 21     % length of buffer; (positive integer)
 22     bufLength = -1;
 23     % dimension of control input; (positive integer)
 24     uDim = -1;
 25     % output value in case the buffer runs empty; (float)
 26     emptyBufOutput = 0;
 27     % current time step; (positive integer)
 28     k = -1;
 29   end
 30   properties  
 31     % variable for debug: colums: time step nr; 
 32     % row 1: time step
 33     % row 2: Received Input Packets - value
 34     % row 3:                        - timeStamp
 35     % row 4 - ? other reived input packets
 36     % row: '----- Buffered:':
 37     %       - value of buffered sequnece
 38     %       - time stamp of buffered sequence
 39     % row: '----- Output':
 40     %       - value of buffered sequnece
 41     %       - time stamp of buffered sequence
 42 
 43     debugInfo;
 44   end
 45   
 46   methods
 47     function s = ClassActuator(bufLength, uDim, varargin)
 48     % ClassActuator(): Creates an actuator object
 49     %
 50     % Synopsis:
 51     % [s] = ClassActuator(bufLength, uDim, emptyBufOutput)
 52     % [s] = ClassActuator(bufLength, uDim)
 53     %
 54     % Input Paramter:
 55     % - bufLength: (positive integer) length of actuator buffer
 56     % - uDim: (postive integer) dimension of single control input of 
 57     %   the plant
 58     % - emptyBufOutput: (vector of dim u) <optional> control input 
 59     %   value in case buffer runs empty
 60     %
 61     % Output Paramter:
 62     % - s: (ClassActuator) initialized actuator
 63       
 64       % -------- 0) check input parameters
 65       % bufLength
 66       if ~isscalar(bufLength)
 67         bufLength %#ok<NOPRT>
 68         error('** Input parameter bufLength is not a scalar **')
 69       elseif(bufLength < 0) || ((bufLength - floor(bufLength)) ~= 0)
 70         bufLength %#ok<NOPRT>
 71         error('** Input parameter bufLength is not a positive integer **')
 72       else
 73         s.bufLength = bufLength;
 74       end
 75       % uDim
 76       if ~isscalar(uDim)
 77         uDim %#ok<NOPRT>
 78         error('** Input parameter uDim is not a scalar **')
 79       elseif(uDim < 0) || ((uDim - floor(uDim)) ~= 0)
 80         uDim %#ok<NOPRT>
 81         error('** Input parameter uDim is not a positive integer **')
 82       else
 83         s.uDim = uDim;
 84       end
 85       % varargin (emptyBufOutput)
 86       if nargin > 4
 87         error(['** DataInDataOut: %d input parameters - ', ...
 88          'only 2 or 3 expected.'], nargin - 1)
 89       end
 90       if nargin == 4
 91         s.emptyBufOutput = varargin{1};
 92         if ~isvector(s.emptyBufOutput)
 93           s.emptyBufOutput 
 94           error('** Wrong format: emptyBufOutput is not a vector **')
 95         elseif( ~isequal(size(s.emptyBufOutput), [s.uDim, 1]) )
 96           s.emptyBufOutput
 97           error('** Dimension mistmatch of emptyBufOutput and uDim **')
 98         end
 99       else
100         % if not specified default value is set to zero
101         s.emptyBufOutput = 0;
102       end
103       % -------- 1) initialize parameters
104       s.bufferedPacket = ClassDataPacket();
105       s.k = 1;
106     end
107     
108     function [uOut] = DataInDataOut(s, numPackets, uPacketsIn)
109     % DataInDataOut: Processing input and output data. To every 
110     % function call 1) new data packets containing control input 
111     % sequences are processed and if a packet with a more recent
112     % time stamp is received than the control input sequence stored in 
113     % the buffer, the new sequence is taken into the buffer.
114     % 2) Output of the actuator is the control input of the buffered 
115     % sequence that corresponds to the actual time step. If there is no
116     % corresponding control input, the default control input determined
117     % by emptyBufOutput is applied.
118     %
119     % Synopsis: 
120     % [uOut] = DataInDataOut(s, numPackets, uPacketsIn)
121     %
122     % Input Paramter:
123     % - s: actuator object;
124     %   (implicit); (ClassActuator)
125     % - numPackets: number of control input sequences that 
126     %   are received by the actuator in the current time step;
127     %   (positive integer)
128     % - uPacketsIn: is one or more data packet(s) that 
129     %   is (are) received by the actuator at the current time step; The
130     %   data packet contains a matrix with control inputs whereby the i-th
131     %   column of the matrix contains a vector with the control inputs
132     %   to be applied to the (timestamp of packet + i)-th time step
133     %   (ClassDataPacket with property value is a matrix with dimension
134     %   s.uDim x s.bufLength)
135     %
136     % Output Paramter:
137     % - uOut: output of the actuator; (vector of dimension s.uDim)
138     %
139     % CONDITION: The function has to be executed every time step      
140 
141       % ------- 0) Check input parameters
142       % numPackets
143       if ~isscalar(numPackets)
144         numPackets %#ok<NOPRT>
145         error('** Input parameter numPackets is not a scalar **')
146       elseif(numPackets < 0) || ...
147             ((numPackets - floor(numPackets)) ~= 0)
148         numPackets %#ok<NOPRT>
149         error('** Input parameter numPackets is not a positive integer **')
150       end
151       % uPacketsIn
152       if numPackets ~= 0
153         for i = 1 : numPackets
154           if ~isa(uPacketsIn(i), 'ClassDataPacket')
155             uPacketsIn(i)
156             error(['** Wrong format: Input parameter <packetIn> is not' ...
157             ' of class ClassDataPacket **'])
158           end
159           if( all(~isnan(uPacketsIn(i).value)) ) 
160             if ~isequal(size(uPacketsIn(i).value), [s.uDim, s.bufLength])
161               s.uDim
162               s.bufLength
163               uPacketsIn(i)
164               error('** Dimension mismatch of uPacketsIn.value **')
165             end
166           % in case only a nan value is received  
167           elseif( (sum(isnan(uPacketsIn(i).value)) ~= 1) || ...
168                   (isequal(size(uPacketsIn(i)), [1, 1])) )
169             i %#ok<NOPRT>
170             uPacketsIn(i).value
171             error('** uPacketsIn.value has empty entries **')
172           end
173         end
174       end
175       % ------- 1) Input data processing 
176       s.debugInfo{1, s.k} = s.k;
177       % NewPacketarrived?
178       if numPackets == 0
179         % no new packet arrived
180         s.debugInfo{2, s.k} = NaN;
181         s.debugInfo{3, s.k} = NaN;
182       else
183         for i = 1 : numPackets
184           % new packet has arrived
185           if(((uPacketsIn(i).timeStamp + s.bufLength) > s.k) &&...
186               (uPacketsIn(i).timeStamp > s.bufferedPacket.timeStamp))
187             % hold new packet in Buffer
188             s.bufferedPacket = uPacketsIn(i);
189           end
190           s.debugInfo{i * 2, s.k} = uPacketsIn(i).value;
191           s.debugInfo{i * 2 + 1, s.k} = uPacketsIn(i).timeStamp;
192         end
193       end
194       % ------- 2) Output data processing 
195       % Data in buffer?
196       if(((s.bufferedPacket.timeStamp + s.bufLength) > s.k) && ...
197           (s.bufferedPacket.timeStamp > 0))
198         uOut = s.bufferedPacket.value(:, ...
199           s.k - s.bufferedPacket.timeStamp + 1);
200       else
201         % if buffer is empty than output default output
202         uOut = s.emptyBufOutput;
203       end
204       % debug info
205       if numPackets == 0
206         s.debugInfo{4, s.k} = '----- Buffered:';
207         s.debugInfo{5, s.k} = s.bufferedPacket.value;
208         s.debugInfo{6, s.k} = s.bufferedPacket.timeStamp;
209         s.debugInfo{7, s.k} = '------ Output:';
210         s.debugInfo{8, s.k} = uOut;
211       else
212         s.debugInfo{(numPackets + 1) * 2, s.k} = '----- Buffered:'; 
213         s.debugInfo{(numPackets + 1) * 2 + 1, s.k} = ...
214           s.bufferedPacket.value;
215         s.debugInfo{(numPackets + 1) * 2 + 2, s.k} = ...
216           s.bufferedPacket.timeStamp;
217         s.debugInfo{(numPackets + 1) * 2 + 3, s.k} = '----- Output:'; 
218         s.debugInfo{(numPackets + 1) * 2 + 4, s.k} = uOut;
219       end
220       % ------- 3) Increment time step
221       s.k = s.k + 1;
222     end
223   end
224 end
  1 classdef ClassCommNetwork < handle
  2   % ClassCommNetwork: This class simulates a communication network to send
  3   % and receive data packets of type ClassDataPacket. A data packet that 
  4   % enters the network will be subject to a stochastic time delays. If the 
  5   % delay is not longer than the maximal allowed delay the packet is 
  6   % relaesed after the delay, else the packet is lost. The time delay
  7   % probabilities and the maximal time delay is configurable. The output 
  8   % of the network depends on the time delays and can be no packet, one 
  9   % packet or more than one packet. 
 10   %
 11   % CONDITION: the function DataInDataOut has to be executed once every 
 12   % time step.
 13   %
 14   % See also ClassDataPacket, ClassActuator
 15   
 16   properties (SetAccess = private)
 17     % cell array with data packets to store input data;
 18     % (array of ClassDataPacket)
 19     nwData = {};
 20     % maximal time delay;
 21     % The network does not output a data packet that has a longer time 
 22     % delay; (positive integer)
 23     maxDelay = -1;
 24     % array with realizations of time delays for all time steps;
 25     % (array of positive integer)
 26     timeDelay =[];
 27     % number of packets that leave the network to current time step;
 28     % Index is the time step; (array of positive integer)
 29     numPacketsOut = [];
 30     % length of simulation time in time steps; (positive integer)
 31     simCycles = -1;
 32     % current time step; (positive integer)
 33     k = -1;
 34     % debug variable
 35     debugInfo = {};
 36   end
 37 
 38   properties(Constant)
 39     % tolerance interval for rounding error
 40     roundingErr = 0.00000001;
 41   end
 42   
 43   methods
 44     function s = ClassCommNetwork(simCycles, maxDelay, delayProb)
 45     % ClassCommNetwork: Constructs object of a communication network
 46     %
 47     % Synopsis:
 48     % [s] = ClassCommNetwork(simCycles, maxDelay, delayProb)
 49     %
 50     % Input Paramter:
 51     % - simCycles: time steps of simulation; (positive integer)
 52     % - maxDelay: maximal time delay possible. The value must be equal to
 53     %   or smaller than the number of entries in the vector <delayProb>;
 54     %   (positive integer)
 55     % - delayProb: describes the delay probabilities. Thereby the i-th
 56     %   entry of delayProb is the  probability that a delay of i time steps
 57     %   occurs. Sum of vector must be equal to 1; 
 58     %   (vector of positive floats)
 59     %   REMARK: If the value maxDelay is smaller than the number of
 60     %   entries in delayProb, then packet losses can occur. 
 61     %
 62     % Output Paramter:
 63     % - s: object of initialized network; (ClassCommNetwork)
 64     %
 65       
 66       % ------- 0) check input parameters and init data:
 67       %delayProb
 68       if size(delayProb, 1) ~= 1
 69         delayProb %#ok<NOPRT>
 70         error('** Input parameter delayProb is no colum vector **')
 71       else
 72         if min(delayProb) < 0
 73           delayProb %#ok<NOPRT>
 74           error('** Input parameter delayProb has negative entries **')
 75         end
 76         if (round(sum(delayProb ./ ClassDelayedKF.roundingErr)) ...
 77             * ClassDelayedKF.roundingErr) ~= 1
 78           delayProb %#ok<NOPRT>
 79           error(['** Sum of entries of input paramter delayProb is' ...
 80             ' not equal to 1 **'])
 81         end
 82       end
 83       % maxDelay
 84       if ~isscalar(maxDelay)
 85         maxDelay %#ok<NOPRT>
 86         error('** Input parameter maxDelay not a scalar **')
 87       elseif( (maxDelay < 0) || ((maxDelay - floor(maxDelay)) ~= 0) )
 88         maxDelay %#ok<NOPRT>
 89         error('** Input parameter maxDelay is not a positive integer **')
 90       elseif find(delayProb, 1, 'last') < maxDelay
 91         % gives index of last non-zero element
 92         maxDelay %#ok<NOPRT>
 93         delayProb %#ok<NOPRT>
 94         error(['** Value of Input parameter maxDelay must be smaller '...
 95           'than or equal to the number of entries in <delayProb> **'])
 96       else
 97         s.maxDelay = maxDelay;
 98       end
 99       % simCycles
100       if ~isscalar(simCycles)
101         simCycles %#ok<NOPRT>
102         error('** Input parameter simCycles not a scalar **')
103       elseif(simCycles < 0) || ((simCycles - floor(simCycles)) ~= 0)
104         simCycles %#ok<NOPRT>
105         error('** Input parameter simCycles is negative **')
106       else 
107         s.simCycles = simCycles;
108       end
109       % init network data
110       s.numPacketsOut = zeros(simCycles, 1);
111       s.nwData = {};
112       % To keep track of packets that will leave the network to the same
113       % time step the network data array is two-dimensional
114       s.nwData{simCycles, 1} = ClassDataPacket();
115       s.k = 1;
116       % Init Time Delays as series for all time steps in advance
117       % before simulation starts. Delays are stochastic described
118       % by vector delayProb. The following function discretesample.m draws 
119       % <simCycle> realizations out of the stochastics given in delayProb
120       s.timeDelay = [];
121       s.timeDelay = discretesample(delayProb, simCycles) - 1;
122     end
123 
124 
125     function [numPackets, packetsOut] = DataInDataOut(s, varargin)
126     % DataInDataOut: Sending and receiving data over the network.
127     % connection. Every function call 1) a new data packet can be sent
128     % over the network (optional) and 2) a data packet leaves the network.
129     %
130     % Synopsis: 
131     % [numPackets, packetsOut] = DataInDataOut(s, packetIn)
132     % [numPackets, packetsOut] = DataInDataOut(s)
133     %
134     % Input Paramter:
135     % - s: network that transmits data;
136     %   (implicet); (ClassCommNetwork)
137     % - packetIn: (optional paramter) is a data packet sent into the
138     %   network; 
139     %   (ClassDataPacket) 
140     %
141     % Output Paramter:
142     % - numPackets: number of packets that leave the network; (integer) 
143     % - packetsOut: datapacket/s that leave the network;
144     %   (array of ClassDataPacket)
145     %
146     % CONDITION: The function has to be executed once every time step
147 
148       % 0) ------ check input parameter
149       if nargin > 2
150         error(['** DataInDataOut: %d input parameters - ', ...
151             'only 0 or 1 expected.'], nargin - 1)
152       end
153       if nargin == 2
154         packetIn = varargin{1};
155         if ~isa(packetIn, 'ClassDataPacket')
156           packetIn  %#ok<NOPRT>
157           error(['** Wrong format: Input parameter <packetIn> is not ' ...
158             'of class ClassDataPacket **'])
159         end
160         packetReceived = true;
161       else
162         packetReceived = false;
163       end
164       % --------- 1) Data in: put new input packet in network data array
165       if(s.timeDelay(s.k) > s.maxDelay) ...
166             || (packetReceived == false)  ...
167             || ((s.k + s.timeDelay(s.k)) > s.simCycles)
168         % packet lost or no new packet available or packet would
169         % leave network after end of simulation
170         % -> do not take packet in data array
171         % debug Info
172         s.debugInfo{1, s.k} = s.k;
173         s.debugInfo{2, s.k} = NaN;
174         s.debugInfo{3, s.k} = s.timeDelay(s.k);
175       else
176         % new packet sent into network:
177         % a) update packet info
178         packetIn.timeStamp = s.k;
179         packetIn.timeDelay = s.timeDelay(s.k);
180         % b) increment packet output counter for the time step when this 
181         % packet will leave
182         i = s.numPacketsOut(s.k + s.timeDelay(s.k)) + 1;
183         s.numPacketsOut(s.k + s.timeDelay(s.k)) = i;
184         % c) write data in network data cell array
185         s.nwData{s.k + s.timeDelay(s.k), i} = packetIn;
186         % Storing data for debug
187         s.debugInfo{1, s.k} = s.k;
188         s.debugInfo{2, s.k} = packetIn.value;
189         s.debugInfo{3, s.k} = s.timeDelay(s.k);
190       end
191       % ----- 2) Data out: write output of network for current time step
192       % first output parameter is number of packets that will leave the
193       % network
194       numPackets = s.numPacketsOut(s.k);
195       % the second output parameter is(are) the packet(s) that will leave
196       % the network to current time step
197       if( numPackets == 0 )
198         % no packet
199         packetsOut = ClassDataPacket();
200         % storing data for debug
201         s.debugInfo{5, s.k} = packetsOut.value;
202         s.debugInfo{6, s.k} = packetsOut.timeStamp;
203         s.debugInfo{7 , s.k} = packetsOut.timeDelay;
204       else
205         % allocate memory for output packets
206         packetsOut(numPackets) = ClassDataPacket();
207         for i = 1:numPackets
208           packetsOut(i) = s.nwData{s.k, i};
209           % storing data for debug
210           s.debugInfo{2 + i * 3, s.k} = packetsOut(i).value;
211           s.debugInfo{3 + i * 3, s.k} = packetsOut(i).timeStamp;
212           s.debugInfo{4 + i * 3, s.k} = packetsOut(i).timeDelay;
213         end
214       end
215       s.debugInfo{4, s.k} = '------------';
216       % ----------- 3) Increment time step
217       s.k = s.k + 1;
218     end
219   end
220 end
  1 classdef ClassDataPacket
  2   % ClassDataPacket: Class defines a data packet used to transmit data over
  3   % a communication network described by the class ClassCommNetwork.
  4   %
  5   % See also ClassCommNetwork, ClassDataPacket
  6 
  7   properties (Access = public)
  8     % time Stamp that indicates time step when data packet was generated;
  9     % (positive integer or NaN)
 10     timeStamp = -1;
 11     % value of data packet. Can be any data; (application dependend data 
 12     % type) 
 13     value = NaN;
 14     % time delay of the data packet, measured in time steps; 
 15     % (positive integer or NaN)
 16     timeDelay = -1;
 17   end
 18 
 19   methods
 20     function s = ClassDataPacket(varargin)
 21     % ClassDataPacket: Creates a Data Packet object
 22     % 
 23     % Synopsis:
 24     % [s] = ClassDataPacket(value, timeStamp, timeDelay)
 25     % [s] = ClassDataPacket()
 26     %
 27     % Input parameter:
 28     % - value: initial value of property value
 29     %   (any type)
 30     % - timeStamp: initial value of property timeStamp
 31     %   (positive Integer)
 32     % - timeDelay: initial value of property timeDelay
 33     %   (positive Integer)
 34     %
 35     % Ouput parameter
 36     % - s: a data packet
 37     %   (ClassDataPacket)
 38       if(nargin == 3)
 39         s.value = varargin{1};
 40         s.timeStamp = varargin{2};
 41         s.timeDelay = varargin{3};
 42       elseif(nargin == 0)
 43         % take default values
 44       else
 45         nargin
 46         error('** wrong number of input arguments **')
 47       end
 48     end
 49 
 50     
 51     function s = set.timeStamp(s, timeStamp)
 52     % set.timeStamp: property set method for property timeStamp.
 53     % This function is automatically called by Matlab if a value is 
 54     % assigned to this propoerty.
 55     % 
 56     % Input parameter:
 57     % - s: data packet object whose timeStamp will be assigned a new value;
 58     %   (implicit); (ClassDataPacket) 
 59     % - timeStamp: new time Stamp of data packet; (positive integer)
 60     %
 61     % Output parameter:
 62     % - s: data packet whose timeStamp was changed;
 63     %   (implicit); (ClassDataPacket)
 64     %
 65       if ~isscalar(timeStamp)
 66         timeStamp %#ok<NOPRT>
 67         error('** Input parameter timeStamp is not a scalar **')
 68       end
 69       if (timeStamp < 0) || ((timeStamp - floor(timeStamp)) ~= 0)
 70         timeStamp %#ok<NOPRT>
 71         error('** Input parameter timeStamp is not a positive integer **')
 72       end
 73       s.timeStamp = timeStamp;
 74     end
 75 
 76     
 77     function s = set.timeDelay(s, timeDelay)
 78     % set.timeDelay: property set method for property timeDelay.
 79     % This function is automatically called by Matlab if a value is 
 80     % assigned to this propoerty.
 81     % 
 82     % Input parameter:
 83     % s: data packet object whose timeDelay will be assigned a new value; 
 84     % (implicit); (ClassDataPacket)
 85     % timeStamp: time Stamp of data packet; (positive integer)
 86     %
 87     % Output parameter:
 88     % - s: data packet whose timeDelay was changed;
 89     %   (implicit); (ClassDataPacket)
 90     %
 91       if ~isscalar(timeDelay)
 92         timeDelay %#ok<NOPRT>
 93         error('** Input parameter timeDelay is not a scalar **')
 94       end
 95       if (timeDelay < 0) || ((timeDelay - floor(timeDelay)) ~= 0)
 96         timeDelay %#ok<NOPRT>
 97         error('** Input parameter timeDelay is not a positive integer **')
 98       end
 99       s.timeDelay = timeDelay;
100     end
101   end
102 end
  1 classdef ClassDelayedIMM < handle
  2   % ClassDelayedIMM: This class represents an estimator for a Networked
  3   % Control System where the control inputs and measurements are subject to
  4   % time-varying delays and stochastic data loss. The estimator basically 
  5   % consist of an interacting multiple-model filter (IMM) with the mode 
  6   % probabilities chosen in respect to the network characteristic. This
  7   % estimates the state of the Networked Control System based on a model of 
  8   % the system, a delay probability distribution of the input network, 
  9   % time-stamped control input sequences (generated by predictive 
 10   % controller) and one or more possilby delayed, time-stamped measurments
 11   % (if any available). The DelayedIMM is a suboptimal filter, since the 
 12   % optimal filter is of infinite dimension.The algorithm is described in 
 13   %
 14   % Joerg Fischer, Achim Hekler, and Uwe D. Hanebeck: "State Estimation in
 15   % Networked Control Systems"; Fusion Conference; 2012
 16   % and be found <a href="matlab: 
 17   % web(['http://Beispiel/ee/ISL/Reprints06/Beispiel' ...
 18   % 'Beispiel' ...
 19   % 'Beispiel'])">here</a>
 20   %
 21   % CONDITIONs: The function Execute of this class has to be executed once
 22   % every time  step.
 23   %
 24   % See also: ClassIMM, ClassCommNetwork, ClassDataPacket
 25 
 26   properties(SetAccess = private)
 27     % dimension of system state; (positive integer)
 28     xDim = 0
 29     o% dimension of system output, i.e. measruements; (positive integer)
 30     yDim = 0
 31     % dimension of input matrix, i.e. single control input; 
 32     % (positive integer)
 33     uDim = 0
 34     % number of control inputs possibly applied to the system;
 35     % this equals the number of maximal considered delay of the input 
 36     % network + 1 for the control input that suffers no delay + 1 for 
 37     % default control input if buffer can run empty (loss rate > 0);
 38     % (positive integer)
 39     maxMode = 0
 40     % finit history of control input sequences applied; 
 41     % rows and columns: received control sequence (row: control
 42     % input vector, columns: Time step when to be applied); 
 43     % 3-dimension: history of received sequences, where newest with index 1
 44     % (3-dim matrix of dimension: s.uDim, s.maxMode, ...
 45     % s.maxDelayY + s.maxMode);
 46     uHis = []
 47     % finit history of measurements received; 
 48     % rows: measurment vector; columns: time step measurement generated;
 49     % non existent measurements are denoted by NaN
 50     % (matrix of dimension: s.yDim, s.maxDelayY);
 51     yHis = []
 52     % Underlying IMM-filter; (ClassIMM)
 53     imm
 54     % estimated mean value; (vector of dimension <xDim>) 
 55     xMean = [];
 56     % error covariance matrix;
 57     % (matrix of dimension <s.xDim> x <s.xDim> of floats)
 58     xErrCov = []
 59     % maximal time delay of mesurements; (positive integer)
 60     maxDelayY
 61     % finite history of former imm states; 
 62     % cell array, where one row is one immState to a certain time step. the
 63     % columns are for different time steps.
 64     % (cell array of dimension s.maxMode x (maxDelayY + 1) of type 
 65     % immState, where immState is struct with fields <pMode>, <xMean> and 
 66     % <xErrCov> that have dimensions according to property defined in 
 67     % ClassIMM)
 68     immHis = {}
 69     % Debug info
 70     DebugS
 71   end
 72   
 73   properties(Constant)
 74     % tolerance interval for rounding error
 75     roundingErr = 0.00000001;
 76   end
 77 
 78   methods
 79     function s = ClassDelayedIMM(A, B, C, wCov, vCov, x0Mean, x0Cov, ...
 80       maxDelayY, delayProbsU)
 81     % ClassDelayedIMM: Creates object of the the delayed IMM filter
 82     %
 83     % Synopsis:
 84     % [s] = ClassDelayedIMM(A, B, C, wCov, vCov, x0Mean, x0Cov, maxDelayY, 
 85     %         delayProbsU)
 86     %
 87     % Input Paramter:
 88     % - A: state matrix of estimatee; (matrix of dimension <xDim> * <xDim>) 
 89     % - B: input matrix of estimatee; (matrix of dimension <xDim> * <uDim>)
 90     % - C: output matrix of estimatee;(matrix of dimension <yDim> * <xDim>) 
 91     % - wCov: covariance matrix of process noise; 
 92     %   (matrix of dimension <xDim> * <xDim>)
 93     % - vCov: covariance matrix of measurement noise; 
 94     %   (matrix of dimension <yDim> * <yDim>) 
 95     % - x0Mean: initial value of mean of state; 
 96     %   (vector of dimension <xDim>) 
 97     % - x0ErrCov: initial value of error covariance matrix;
 98     %   (matrix of dimension <xDim> * <xDim>)
 99     % - maxDelayY: maximal time delay of mesurements that can be considered 
100     %   by the filter. Measurements that were subject to longer time delays
101     %   are ignored; 
102     %   (positive integer)
103     %   REMARK: The higher maxDelayY the higher the required memory and
104     %           and computational load
105     % - delayProbsU: describes the probabilities of control input time
106     %   delays. Thereby the i-th entry of delayProb is the probability that
107     %   a delay of i time steps occurs; 
108     %   Sum of vector must be equal to 1;
109     %   (vector of positive floats)
110     %
111     % Output Paramter:
112     % - s: created delayed IMM filter object; (ClassDelayedIMM) 
113 
114       % ---- check input paramters
115       % system matrices:
116       % A
117       [d1 d2] = size(A);
118       if(d1 ~= d2)
119         A  %#ok<NOPRT>
120         error('** Wrong dimension of input paramter A **')
121       else
122         s.xDim = d1;
123       end
124       % B
125       [d1 d2] = size(B);
126       if (d1 ~= s.xDim)
127         A %#ok<NOPRT>
128         B %#ok<NOPRT>
129         error('** Dimension mismatch matrix A and B **')
130       else
131         s.uDim = d2;
132       end
133       % C
134       [d1 d2] = size(C);
135       if (d2 ~= s.xDim)
136         A %#ok<NOPRT>
137         C %#ok<NOPRT>
138         error('** Dimension mismatch matrix A and C **')
139       else
140         s.yDim = d1;
141       end
142       
143       % maxDelayY
144       if ~isscalar(maxDelayY)
145         maxDelayY %#ok<NOPRT>
146         error('** Input parameter maxDelayY not a scalar **')
147       elseif( (maxDelayY < 0) || ((maxDelayY - floor(maxDelayY)) ~= 0) )
148         maxDelayY %#ok<NOPRT>
149         error('** Input parameter maxDelayY is not a positive integer **')
150       else
151         s.maxDelayY = maxDelayY;
152       end
153 
154       % delayProbsU
155       if size(delayProbsU, 1) ~= 1
156         delayProbsU %#ok<NOPRT>
157         error('** Input parameter delayProbsU is not a row vector **')
158       elseif any(delayProbsU < 0)
159           delayProbsU %#ok<NOPRT>
160           error('** Input parameter delayProbsU has negative entries **')
161       elseif (round(sum(delayProbsU ./ ClassDelayedKF.roundingErr)) ...
162             * ClassDelayedKF.roundingErr) ~= 1
163         delayProbsU %#ok<NOPRT>
164         error(['** Sum of entries of input paramter delayProbsU is' ...
165           ' not equal to 1 **'])
166       end
167       s.maxMode = numel(delayProbsU);
168 
169       % init control input sequence history
170       s.uHis = zeros(s.uDim, s.maxMode, s.maxDelayY + s.maxMode);
171       % init measurement history
172       s.yHis = nan(s.yDim, s.maxDelayY);
173       % init transition matrix
174       trans = calcDelayTrans(delayProbsU);
175       % init IMM
176       % (formats of wCov, vCov, x0Mean and x0Cov are tested inside 
177       % ClassIMM())
178       s.imm = ClassIMM(A, B, C, wCov, vCov, x0Mean, x0Cov, trans);
179       % init IMM histroy for IMM-state recovery
180       for delay = 1 : (s.maxDelayY + 1)
181         for mode = 1 : s.maxMode
182           s.immHis{mode, delay} = s.imm.immState{mode, 1};
183         end
184       end
185     end
186     
187     
188     function xMean = Execute(s, uInput, nrMeasPackets, varargin)
189     % Execute: Function executes one time step of the time delayed 
190     % IMM filter and calculates the estimated state of the considered
191     % system. Inputs of the function are the control input sequence
192     % generated by the controller and (if available) one or more 
193     % data packets containing the information about the measurements.
194     %
195     % Synopsis: 
196     % [xMean] = Execute(s, uInput, nrMeasPackets, measPacketIn)
197     % [xMean] = Execute(s, uInput, 0)
198     %
199     % Input Paramter:
200     % - s: object of delayed IMM filter to be executed;
201     %   (implicit); (ClassDelayedIMM) 
202     % - uInput: matrix or data packet with control input sequence sent to
203     %   actuator;
204     %   (matrix of dimension <s.uDim> x <s.maxMode>) or 
205     %   (ClassdataPacket with property value is a matrix of dimension 
206     %   <s.uDim> x <s.maxMode>)
207     %   REMARK: This is NOT a vector with control inputs possibly active
208     %   at the current time step. The filter calculates these out of the
209     %   control input sequences.
210     % - nrMeasPackets: number of measurement packets received by the
211     %   delayed IMM-filter. If the value is zero than the parameter
212     %   measPacketIn is optional and can be omitted; 
213     %   (positive integer)
214     % - measPacketIn: One or more time-stamped measurements of the state of 
215     %   the system; if nrMeasPackets is 0 than this parameter is not
216     %   checked and can be any size or even omitted.
217     %   (array with dimension <nrMeasPackets> and of type ClassDataPacket)
218     %   or (any format if nrMeasPackets = 0)
219     %
220     % Output Paramter:
221     % - xMean: estimated state of the system; 
222     %   (vector of dimension <s.xDim>)
223     %
224     % CONDITION: The function has to be executed once every time step 
225 
226       %% 0) ----- check input parameter
227       if nargin > 4
228         error(['** Execute: %d input parameters - ', ...
229             'only 2 or 3 expected.'], nargin - 1)
230       end
231       % nrMeasPackets
232       if ~isscalar(nrMeasPackets)
233         nrMeasPackets %#ok<NOPRT>
234         error('** Input parameter nrMeasPackets not a scalar **')
235       elseif(nrMeasPackets < 0) || ...
236           ((nrMeasPackets - floor(nrMeasPackets)) ~= 0)
237         nrMeasPackets %#ok<NOPRT>
238         error(['** Input parameter nrMeasPackets is not a positive '...
239           'integer **'])
240       end
241       % measPacketIn
242       if(nrMeasPackets ~= 0) 
243         if(nargin ~= 4)
244           nrMeasPackets %#ok<NOPRT>
245           error('** Execute: input parameter <measPacketIn> is missing')
246         else
247           measPacketIn = varargin{1};
248           for i = 1 : nrMeasPackets
249             if ~isa(measPacketIn(i), 'ClassDataPacket')
250               measPacketIn(i)
251               error(['** Input parameter measPacketIn is not of type <' ... 
252                 'ClassDataPacket> **'])
253             elseif( any(isnan(measPacketIn(i).value)) ) 
254               measPacketIn(i)
255               error(['** At least one entry of value property of the ' ...
256                 'data packet <measPacketIn.value> is empty **'])
257             elseif( ~isequal(size(measPacketIn(i).value), [s.yDim, 1]) )
258               measPacketIn(i)
259               error('** Dimension mismatch of measPacketIn.value **')
260             end
261           end
262         end
263       end
264       
265       % uInput
266       % can be raw control sequence or being embedded in a Data Packet
267       if isa(uInput, 'ClassDataPacket')
268         if any(any(isnan(uInput.value)))
269            error('** Property <value> of data packet has empty entries **')
270         end
271         if( ~isequal(size(uInput.value), [s.uDim, s.maxMode]) )
272            uInput  %#ok<NOPRT>
273            uInput.value
274            error(['** Dimension of <value> of data packet <uInput> ' ...
275              'is not equal to [%d, %d] **'], s.uDim, s.maxMode)
276         end
277         uSeq = uInput.value;
278       elseif( isequal(size(uInput), [s.uDim, s.maxMode]) )
279         uSeq = uInput;
280       else
281         uInput  %#ok<NOPRT>
282         error(['** Input parameter uSeq is neither a suitable vector'...
283         ' nor a suitable data packet of type ClassDataPacket **']);
284       end
285       
286       %% ------ 1) update measurement history
287       % a) time shift of history because of new time step
288       s.yHis = [nan(s.yDim, 1), s.yHis(:, 1:end-1)];
289       % b) incorporation of received measurements
290       if( nrMeasPackets ~= 0 )
291         for i = 1 : nrMeasPackets
292           s.yHis(:, measPacketIn(i).timeDelay + 1) = measPacketIn(i).value;
293         end
294       end
295 
296       %% --------2) update input sequence history
297       s.uHis(:, :, 2:end) = s.uHis(:, :, 1:(end - 1));
298       s.uHis(:, :, 1) = uSeq;
299       % debug information
300       DebugUHis(:, :) = s.uHis(1, :, :);
301       for i = 1:s.maxMode
302         for j = 1:s.maxDelayY
303           s.DebugS(i, j) = s.immHis{i, j}.pMode;
304           s.DebugS(i + s.maxMode, j) = s.immHis{i, j}.xMean(1);
305         end
306       end
307 
308       %% -------- 3) re-looping IMM for delayed measurements
309       % if a delayed measurement is received, the IMM-algorithm
310       % will be recomputed beginning by the time step the received
311       % measurement was generated.
312       % a) get longest delay of the measurements received in this time step
313       if(nrMeasPackets ~= 0) 
314         measMaxDelay = 0;
315         for i = 1 : nrMeasPackets
316           if( (measPacketIn(i).timeDelay > measMaxDelay) && ...
317               (measPacketIn(i).timeDelay <= s.maxDelayY) )
318             measMaxDelay = measPacketIn(i).timeDelay ;
319           end
320         end
321         % b) if there was at least one delayed measurement, then recover
322         % the state of the IMM to that time step
323         if(measMaxDelay > 0)
324           if measMaxDelay > s.maxDelayY
325           measMaxDelay
326           end
327           s.imm.ReInit(s.immHis(:, measMaxDelay + 1));
328         end
329       else
330         measMaxDelay = 0;
331       end
332       % c) execute the underlying IMM-algorithm beginning with the time
333       % step the oldest received measurement was generated
334       for delayStep = measMaxDelay : - 1 : 0
335         % compute control inputs that could have been active to that time 
336         % step by using the control input history
337         uRec = [];
338         for i = 1 : s.maxMode
339           uRec = [uRec, s.uHis(:, i, i + delayStep)];
340         end
341         s.imm.Execute(uRec, s.yHis(:, delayStep + 1));
342         % updating imm history
343         % (in column 1 is the current imm state)
344         % in column 2 the state at the begining of last time instant and 
345         % so on
346         if(delayStep ~= 0)
347           % overwriting old history
348           s.immHis(:, delayStep) = s.imm.immState;
349         else
350           % incorporate new state (in colum 1 is the current state)
351           s.immHis = [s.imm.immState, s.immHis(:, 1 : end - 1)];
352         end
353       end
354 
355       %% -------- 3) setting output
356       % saving results and setting output
357       s.xErrCov = s.imm.xErrCov;
358       s.xMean   = s.imm.xMean;
359       xMean = s.xMean;
360       % debugInfo
361       for i = 1:s.maxMode
362         for     j = 1:s.maxDelayY
363           s.DebugS(i, j) = s.immHis{i, j}.pMode;
364           s.DebugS(i + s.maxMode, j) = s.immHis{i, j}.xMean(1);
365         end
366       end
367     end
368   end
369 end
  1 classdef ClassDelayedKF < handle
  2   % ClassDelayedKF: This class represents a kalman-filter that can operate
  3   % with time delayed control inputs and/or time delayed measurements. The 
  4   % delayed kalman-filter calculates the linear minimum mean square state 
  5   % estimate of a system based on a model of that system, the expected 
  6   % control input applied to the system and possibly time-delayed 
  7   % measurements of the state.
  8   %
  9   % The algorithm is described in
 10   % Maryam Moayedia, Yung Kuan Foob, Yeng Chai Soha; Filtering for
 11   % networked control systems with single/multiple measurement packets
 12   % subject to multiple-step measurement delays and multiple packet
 13   % dropouts; International Journal of Systems Science; 2011
 14   % and can be found  <a href="matlab:
 15   % web('http://www.tandfonline.com/doi/abs/10.1080/00207720903513335')"
 16   % >here</a>
 17   %
 18   % CONDITION: The function Execute has to be executed once every time 
 19   % step.
 20   
 21   %#ok<*NOPRT>
 22   properties(SetAccess = private)
 23     % estimated mean value; (vector of dimension <xDim> and type float) 
 24     xMean = [];
 25     % error covariance matrix;
 26     % (matrix of dimension <xDim> x <xDim> of floats)
 27     xErrCov = [];
 28     % state matrix of estimatee;
 29     % (matrix of dimension <xDim> x <xDim> of floats)
 30     A = [];
 31     % input matrix of estimatee;
 32     % (matrix of dimension <uDim> x <uDim> of floats) 
 33     B = [];
 34     % output matrix of estimatee;
 35     % (matrix of dimension <uDim> x <uDim> of floats)
 36     C = [];
 37     % dimension of estimatee state; (positive integer)
 38     xDim = 0;
 39     % dimension of output; (positive integer)
 40     yDim = 0;
 41     % dimension of input; (positive integer)
 42     uDim = 0;
 43     % covariance matrix of measurement noise; 
 44     % (matrix of dimension <yDim> x <yDim> of floats)
 45     vCov = [];
 46     % covariance matrix of process noise; 
 47     % (matrix of dimension <xDim> x <xDim> and of floats)
 48     wCov = [];
 49     % augmented system matrix; 
 50     % (matrix of dimension (xdim * (ymaxDel + 1)) x (xdim * (ymaxDel + 1))
 51     AAug = [];
 52     % augmented process noise input Matrix
 53     % (matrix of dimension (xdim * (ymaxDel + 1)) x (xdim * (ymaxDel + 1))
 54     BwAug = [];
 55     % augmented state vector
 56     % (vector of dimension s.xDim x (s.yMaxDel + 1), 1)
 57     xAug = [];
 58     % augmented measurement noise covariance
 59     % (matrix of dimension s.yDim x (s.yMaxDel + 1))
 60     vAugCov = [];
 61     % augmented error covariance
 62     % (matrix of dimension (s.xDim x (s.yMaxDel + 1))^2)
 63     errAugCov = [];
 64     % stacked augmented output matrix
 65     % (3-dim array of dim: s.yDim, s.xDim x (s.yMaxDel + 1), s.yMaxDel + 1)
 66     arrC = [];
 67     % stacked augmented Measurement Noise Selection Matrix
 68     % (3-dim array of dim: s.yDim, s.yDim x (s.yMaxDel + 1), s.yMaxDel + 1)
 69     arrD = [];
 70     % helping matrix
 71     % matrix of dimension: (s.xDim x (s.yMaxDel + 1) , s.yDim)
 72     Fs = [];
 73     % helping matrix
 74     % matrix of dimension: (s.xDim, (s.yMaxDel + 1) s.xDim)
 75     gamma = [];
 76     % delay dependend Kalman gain
 77     % (matrix of dimension s.xDim x s.yDim)
 78     Ls = [];
 79     % maximal time delay of mesurements; (positive integer)
 80     yMaxDel = -1;
 81     % number of control inputs possibly applied to the system
 82     % (positive integer)
 83     uSeqLength = -1;
 84     % weighting factors to calculate expected control inpput
 85     % (vector of dimension s.uSeqLength)
 86     weight = [];
 87     % finit history of control input sequences applied
 88     % (3-dim matrix of dimension: (s.uDim, s.uSeqLength, s.uSeqLength)
 89     uhis = [];
 90   end
 91   properties(Constant)
 92     % tolerance interval for rounding error
 93     roundingErr = 0.00000001;
 94   end
 95 
 96   methods
 97     function s = ClassDelayedKF(A, B, C, wCov, vCov, x0Mean, ...
 98         x0ErrCov, yMaxDel, varargin)
 99     % ClassDelayedKF: Creates object of a delayed Kalman filter
100     %
101     % Synopsis:
102     % [s] = ClassDelayedKF(A, B, C, wCov, vCov, x0Mean, x0Cov, yMaxDel, 
103     %                      uDelProbs)
104     % [s] = ClassDelayedKF(A, B, C, wCov, vCov, x0Mean, x0Cov, yMaxDel) 
105     %
106     % Input Paramter:
107     % - A: state matrix of estimatee; (matrix of dimension <xDim> * <xDim>) 
108     % - B: input matrix of estimatee; (matrix of dimension <xDim> * <uDim>)
109     % - C: output matrix of estimatee;(matrix of dimension <yDim> * <xDim>) 
110     % - wCov: covariance matrix of process noise; 
111     %   (matrix of dimension <xDim> * <xDim>)
112     % - vCov: covariance matrix of measurement noise; 
113     %   (matrix of dimension <yDim> * <yDim>) 
114     % - x0Mean: initial value of expected mean value; 
115     %   (vector of dimension <xDim> and type float) 
116     % - x0ErrCov: initial value of error covariance matrix;
117     %   (matrix of dimension <xDim> * <xDim>)
118     % - yMaxDel: maximal time delay of mesurements that can be considered 
119     %   by the filter. Measurements that were subject to longer time delays
120     %   are ignored; (positive integer)
121     %   REMARK: The higher yMaxDel the higher the required memory because
122     %           the dimension of the augmented matrices are dependend of
123     %           yMaxDel
124     % - uDelProbs: (optional parameter) describes the probabilities of 
125     %   control input time delays. Thereby the i-th entry of delayProb is 
126     %   the probability that a delay of i time steps occurs. Sum of vector 
127     %   must be equal to 1; If parameter is omitted it is set to 1 and so
128     %   that control input is deterministic; 
129     %   (vector of positive floats)
130     %
131     % Output Paramter:
132     % - s: created delayed Kalman filter object; (ClassDelayedKF) 
133       
134       % -------- check input parameter and init object variables -----
135       if nargin > 9
136         error('** Too many input arguments **')
137       end
138       % system model
139       [d1 d2] = size(A);
140       if( d1 ~= d2 )
141         disp(A)
142         error('** Wrong dimension of input paramter A **')
143       else
144         s.A = A;
145         s.xDim = d1;
146       end
147       [d1 d2] = size(B);
148       if( d1 ~= s.xDim )
149         disp(A)
150         disp(B)
151         error('** Dimension mismatch matrix A and B **')
152       else
153         s.B = B;
154         s.uDim = d2;
155       end
156       [d1 d2] = size(C);
157       if( d2 ~= s.xDim )
158         disp(A)
159         disp(C)
160         error('** Dimension mismatch matrix A and C **')
161       else
162         s.C = C;
163         s.yDim = d1;
164       end
165       % initial state values
166       if( isequal(size(x0Mean), [s.xDim, 1]) )
167         s.xMean = x0Mean;
168       else
169         disp(x0Mean)
170         error('** Wrong parameter dimension x0Mean **')
171       end
172       if( isequal(size(x0ErrCov), [s.xDim, s.xDim]) )
173         s.xErrCov = x0ErrCov;
174       else
175         disp(x0ErrCov)
176         error('** Wrong parameter dimension x0Cov **')
177       end
178       % maximum delay of measurements
179       if ~isscalar(yMaxDel)
180         disp(yMaxDel)
181         error('** Input parameter yMaxDel not a scalar **')
182       elseif( (yMaxDel < 0) || ((yMaxDel - floor(yMaxDel)) ~= 0) )
183         disp(yMaxDel)
184         error('** Input parameter yMaxDel is not a positive integer **')
185       else
186         s.yMaxDel = yMaxDel;
187       end
188       % process noise covariances
189       if( isequal(size(wCov), [s.xDim, s.xDim]) )
190         s.wCov = wCov;
191       else
192         disp(A)
193         disp(wCov)
194         error('** Wrong parameter dimension wCov **')
195       end
196       % measurement noise covariances
197       if( isequal(size(vCov), [s.yDim, s.yDim]) )
198         s.vCov = vCov;
199       else
200         disp(C)
201         disp(vCov)
202         error('** Wrong parameter dimension vCov **')
203       end
204       s.vAugCov = kron(eye(s.yMaxDel+ 1, s.yMaxDel+ 1), s.vCov);
205       % check input parameter uDelayProbs
206       if nargin == 9
207         uDelProbs = varargin{1};
208         if size(uDelProbs, 1) ~= 1
209           disp(uDelProbs)
210           error('** Input parameter uDelProbs is not a row vector **')
211         elseif min(uDelProbs) < 0
212             disp(uDelProbs)
213             error('** Input parameter uDelProbs has negative entries **')
214         elseif (round(sum(uDelProbs ./ ClassDelayedKF.roundingErr)) ...
215             * ClassDelayedKF.roundingErr) ~= 1
216           disp(uDelProbs)
217           error(['** Sum of entries of input paramter uDelProbs is' ...
218             ' not equal to 1 **'])
219         end
220       else
221         uDelProbs = 1;
222       end
223       s.uSeqLength = numel(uDelProbs);
224       % init control input sequence history
225       s.uhis = zeros(s.uDim, s.uSeqLength, s.uSeqLength);
226       
227       % ---- init augmented system matrices ----
228       % stacked augmented output matrix
229       s.arrC = zeros(s.yDim, s.xDim * (s.yMaxDel + 1), s.yMaxDel + 1);
230       for counter = 1:1:(s.yMaxDel + 1)
231         s.arrC(:, :, counter) = [zeros(s.yDim, s.xDim * (counter - 1)), ...
232           s.C, zeros(s.yDim, s.xDim * (s.yMaxDel + 1 - counter))];
233       end
234       % stacked augmented Measurement Noise Selection Matrix
235       s.arrD = zeros(s.yDim, s.yDim * (s.yMaxDel + 1), s.yMaxDel + 1);
236       for counter = 1:1:(s.yMaxDel + 1)
237         s.arrD(:, :, counter) = [zeros(s.yDim, s.yDim * (counter - 1)), ...
238           eye(s.yDim), zeros(s.yDim, s.yDim * (s.yMaxDel + 1 - counter))];
239       end
240       % augmented error covariance
241       s.errAugCov = zeros(s.xDim * (s.yMaxDel + 1), s.xDim * ...
242         (s.yMaxDel + 1));
243       s.errAugCov(:,:) = kron(ones(s.yMaxDel + 1, s.yMaxDel + 1), s.xErrCov);
244       % augmented state vector
245       s.xAug = zeros(s.xDim * (s.yMaxDel + 1), 1);
246       s.xAug = kron(ones(s.yMaxDel + 1, 1), s.xMean);
247       % init helping matrices
248       s.Fs = zeros(s.xDim * (s.yMaxDel + 1) , s.yDim);
249       s.gamma = [eye(s.xDim, s.xDim), zeros(s.xDim, s.xDim * s.yMaxDel)];    
250       % augmented system Matrix
251       s.AAug = [s.A, zeros(s.xDim, s.xDim * s.yMaxDel); kron(eye(...
252         s.yMaxDel), eye(s.xDim)), zeros(s.xDim * s.yMaxDel, s.xDim)];
253       % augmented process noise input Matrix
254       s.BwAug = [eye(s.xDim); zeros(s.yMaxDel * s.xDim, s.xDim)];
255 
256       % ------ init weighting factors for later cyclic calculation of 
257       % weighted averaged input. The filter uses the steady state 
258       % approximation of the occuring delays. Therefore the occuring delays 
259       % are described as a Markov chain with state r_k and the steady state 
260       % solution is the solution of the equilibrium equation 
261       % r_k = trans_matrix * r_k 
262       % with trans_matrix the delay probability transition matrix.
263       % 1) generating the transition matrix
264       trans = calcDelayTrans(uDelProbs);
265       % 2)  deriving weightings as steady state solution of transistion
266       % matrix
267       % steady state solution is eigenvector of eigenvalue 1
268       [eigenVec, eigenVal] = eig(trans');
269       % bounds have to be inserted because of rounding errors
270       [row, ~] = find((eigenVal > (1 - ClassDelayedKF.roundingErr)) & ...
271         (eigenVal < (1 + ClassDelayedKF.roundingErr)));
272       if isempty(row)
273         error(['** Transition matrix for time delays has no steady' ...
274           'state solution. Please check delay probabilities! **']);
275       else
276         % weightings are the normalized eigenvector
277         s.weight = eigenVec(:, row) ./ sum(eigenVec(:, row));
278       end
279     end
280     
281     
282     function [xMean] = Execute(s, uInput, nrMeasPackets, varargin)
283     % Execute: Function executes one time step of the time delayed 
284     % Kalman filter and calculates the estimated state of the considered
285     % system. Inputs of the function are the control input sequence
286     % generated by the controller and (if available) one or more 
287     % data packets containing the information about the measurements.
288     %
289     % Synopsis: 
290     % [xMean] = Execute(s, uInput, nrMeasPackets, measPacketIn)
291     % [xMean] = Execute(s, uInput, 0)
292     %
293     % Input Paramter:
294     % - s: object of delayed Kalman filter to be executed;
295     %   (implicit); (ClassDelayedKF) 
296     % - uInput: matrix or data packet with control input sequence sent to
297     %   actuator;
298     %   (matrix of dimension <s.uDim> x <s.uSeqLength>) or 
299     %   (ClassdataPacket with property value is a matrix of dimension 
300     %   <s.uDim> x <s.uSeqLength>)
301     %   REMARK: This is NOT a vector with control inputs possibly active
302     %   at the current time step. The filter calculates these out of the
303     %   sequences.
304     % - nrMeasPackets: number of measurement packets received by the
305     %   delayed Kalman filter. If the value is zero than the parameter
306     %   measPacketIn is optional and can be omitted; 
307     %   (positive integer)
308     % - measPacketIn: One or more measurements of the state of the system;
309     %   if nrMeasPackets is 0 than this parameter is not considered or
310     %   checked and can be any size.
311     %   (array with dimension <nrMeasPackets> and of type ClassDataPacket)
312     %   or (any format/content if nrMeasPackets = 0)
313     %
314     % Output Paramter:
315     % - xMean: estimated state of the system; 
316     %   (vector of dimension <s.xDim> and type float)
317     %
318     % CONDITION: The function has to be executed once every time step      
319       
320       % ------ 0) check input parameter
321       if nargin > 4
322         error(['** Execute: %d input parameters - ', ...
323             'only 2 or 3 expected.'], nargin - 1)
324       end
325       % nrMeasPackets
326       if ~isscalar(nrMeasPackets)
327         disp(nrMeasPackets)
328         error('** Input parameter nrMeasPackets not a scalar **')
329       elseif(nrMeasPackets < 0) || ...
330           ((nrMeasPackets - floor(nrMeasPackets)) ~= 0)
331         disp(nrMeasPackets)
332         error(['** Input parameter nrMeasPackets is not a positive '...
333           'integer **'])
334       end
335       % measPacketIn
336       if(nrMeasPackets ~= 0) 
337         if(nargin ~= 4)
338           disp(nrMeasPackets)
339           error('** Execute: input parameter <measPacketIn> is missing')
340         else
341           measPacketIn = varargin{1};
342           for i = 1 : nrMeasPackets
343             if ~isa(measPacketIn(i), 'ClassDataPacket')
344               disp(measPacketIn(i))
345               error(['** Input parameter measPacketIn is not of type <' ... 
346                 'ClassDataPacket> **'])
347             elseif( any(isnan(measPacketIn(i).value)) ) 
348               disp(measPacketIn(i))
349               error(['** At least one entry of value property of the ' ...
350                 'data packet <measPacketIn.value> is empty **'])
351             elseif( ~isequal(size(measPacketIn(i).value), [s.yDim, 1]) )
352               disp(measPacketIn(i))
353               error('** Dimension mismatch of measPacketIn.value **')
354             end
355           end
356         end
357       end
358       % uInput
359       if isa(uInput, 'ClassDataPacket')
360         if any(any(isnan(uInput.value)))
361            error('** Property <value> of data packet has empty entries **')
362         end
363         if( ~isequal(size(uInput.value), [s.uDim, s.uSeqLength]) )
364            disp(uInput)
365            disp(s.uDim)
366            disp(s.uSeqLength)
367            error(['** Dimension mismatch of <value> of data packet ' ...
368               '<uInput> **'])
369         end
370         uSeq = uInput.value;
371       elseif( isequal(size(uInput), [s.uDim, s.uSeqLength]) )
372         uSeq = uInput;
373       else
374         disp(uInput)
375         error(['** Input parameter uSeq is neither a sutiable vector'...
376         ' nor a suitable data packet of type ClassDataPacket**']);
377       end
378       
379       % ------- 1) Prediction (always performed)
380       s.Predict(uSeq);
381       % ------- 2) Measurement Update
382       if( nrMeasPackets == 0 )
383         %lost measurement --> no measurement update
384       else
385         % Processing all measurements
386         for i = 1 : nrMeasPackets
387           if( measPacketIn(i).timeDelay <= s.yMaxDel )
388             % delay smaller than maximal delay --> measurement usable
389             s.MeasUpdate(measPacketIn(i).value, measPacketIn(i).timeDelay);
390           end
391         end
392       end
393       xMean = s.xMean;
394     end
395   end
396   
397   methods(Access = private)
398     function Predict(s, uSeq)
399     % Predict: Performs prediction step of the time delayed Kalman filter.
400     %
401     % Synopsis: 
402     % Predict(s, uSeq)
403     %
404     % Input Paramter:
405     % - s: object of time delayed Kalman filter whose prediction step will
406     %   be executed; 
407     %   (implicit); (ClassDelayedKF) 
408     % - uSeq: matrix of control input sequence sent to actuator;
409     %   (vector of dimension <s.uDim> x <s.uSeqLength>)
410     %
411     % Output Parameter:
412     % - none
413    
414       % 1) calculate expected value of applied control input
415       s.uhis(:, :, 2:end) = s.uhis(:, :, 1 : (end - 1));
416       s.uhis(:, :, 1) = uSeq;
417       uExp = zeros(s.uDim, 1);
418       for counter = 1 : 1 : s.uSeqLength
419         uExp = uExp + s.uhis(:, counter, counter) .* s.weight(counter);
420       end
421       
422 %       s.uhis = [uSeq'; s.uhis(1 : s.uDim * (s.uSeqLength - 1), :)];
423 %       uExp = zeros(1, s.uDim);
424 %       for counter = 1 : 1 : s.uSeqLength
425 %         uExp = uExp + s.uhis(counter, ((counter - 1) * s.uDim) + 1:...
426 %           (counter * s.uDim)) .* s.weight(counter);
427 %       end
428 %       uExp = uExp';
429       % 2) calculate state error due to uncertainty about control input
430       uErrCov = zeros(s.xDim * (s.yMaxDel + 1));
431       for counter = 1:s.uSeqLength
432         uErrCov = uErrCov + s.weight(counter) .* s.BwAug * s.B * ...
433           (s.uhis(:, counter, counter) - uExp) * ...
434           (s.uhis(:, counter, counter) - uExp)' * s.B' * s.BwAug';
435 %         uErrCov = uErrCov + s.weight(counter) .* s.BwAug * s.B * ...
436 %           ((s.uhis(counter, ((counter - 1) * s.uDim) + 1 : ...
437 %           (counter * s.uDim)))' - uExp) * ...
438 %           ((s.uhis(counter, ((counter - 1) * s.uDim) + 1 : ...
439 %           (counter * s.uDim)))' - uExp)' * s.B' * s.BwAug';
440        end
441       % 3) use 1 and 2 to perfrom prediction algorithm
442       s.xMean = s.A * s.xMean + s.B * uExp;
443       s.xAug = [s.xMean; s.xAug(1:(end - s.xDim))];
444       s.errAugCov = s.AAug * s.errAugCov * s.AAug' + uErrCov  ...
445         + s.BwAug * s.wCov * s.BwAug';
446     end
447 
448     function MeasUpdate(s, yValue, timeDelay)
449     % MeasUpdate: Performs filter step of the time delayed Kalman filter.
450     %
451     % Synopsis: 
452     % MeasUpdate(s, yValue, timeDelay)
453     %
454     % Input Paramter:
455     % - s: object of time delayed Kalman filter whose prediction step will
456     %   be executed; 
457     %   (implicit); (ClassDelayedKF) 
458     % - yValue: measurement value
459     %   (vector of dimension <s.yDim> of type float)
460     % - timeDealy: time step when measurement was generated
461     %   (positive integer)
462     %
463     % Output Parameter:
464     % - none
465 
466       % ----- 1) calculated filtered mean value
467       d = timeDelay + 1;
468       s.Ls = s.gamma * s.errAugCov * s.arrC(:,:,d)' * ...
469         inv( s.arrC(:,:,d) * s.errAugCov * s.arrC(:,:,d)' +...
470         s.arrD(:,:,d) * s.vAugCov * s.arrD(:,:,d)');
471       s.xMean = s.xMean + s.Ls * (yValue - s.arrC(:,:,d) * s.xAug);
472       s.xAug(1:s.xDim) = s.xMean;
473       % ----- 2) calculated new augmented error covariance matrix
474       s.Fs = [s.Ls; zeros(s.xDim * s.yMaxDel, s.yDim)];
475       s.errAugCov = (eye(s.xDim * (s.yMaxDel + 1)) - ...
476         s.Fs * s.arrC(:,:,d)) * s.errAugCov * (eye(s.xDim * ...
477         (s.yMaxDel + 1)) - s.Fs * s.arrC(:,:,d))' + s.Fs * ...
478         s.arrD(:,:,d) * s.vAugCov * s.arrD(:,:,d)' * s.Fs';    
479     end
480   end
481 end
482 
483 % For Debug: this calculates the weighting factors directly for a uniform
484 % distribution.
485 % if s.uSeqLength == 1
486 % s.debug2 = 1;
487 % else
488 % for counter = 1 : 1 : s.uSeqLength
489 %     s.debug2(counter) = 1;
490 %     for counter2 = 1 : (counter - 1)
491 %         s.debug2(counter) = s.debug2(counter) * ...
492 %             (1 - counter2 / s.uSeqLength);
493 %     end
494 %     s.debug2(counter) = s.debug2(counter) * counter  / s.uSeqLength;
495 % end
496 % end
  1 classdef ClassIMM < handle
  2   % ClassIMM: This class represents an interacting multiple-model filter
  3   % (IMM). The IMM approximates a minimum mean square state
  4   % estimate of a Markovian jump linear system (MJLS) based on a model of 
  5   % the MJLS, the control inputs possibly applied to the models and 
  6   % a measurment (if available). The IMM is a suboptimal filter, however,
  7   % the optimal filter would be a Kalman filter bank of infinite 
  8   % dimension and therefore, is not implementable.
  9   % The algorithm is described in table II of
 10   %
 11   % Li, X.R. and Jilkov, V.P. A survey of maneuvering target tracking—Part
 12   % V: Multiple-model methods; Conference on Signal and Data Processing of
 13   % Small Targets; 2003
 14   % and be found <a href="matlab: 
 15   % web(['http://coe.uno.edu/ee/ISL/Reprints06/Survey%20of%2' ...
 16   % '0Maneuvering%20Target%20Tracking%20-%20Part%20V%20Multiple-Model' ...
 17   % '%20Methods.pdf'])">here</a>
 18   %
 19   % CONDITION: The function Execute of this class has to be executed once
 20   % every time  step.
 21   %
 22   % See also:
 23 
 24   
 25   properties(SetAccess = private)
 26     % transition matrix of Markov chain governing the delay probabilities;
 27     % row = oldmode, column = newmode;
 28     % (matrix of dimension <s.maxMode> x <s.maxMode>) with sum(row) = 1
 29     trans = []
 30     % current mode probabilites; (column vector of dimension <s.maxMode>
 31     pMode = []
 32     % predicted mode probabilites; (column vector of dimension <s.maxMode>
 33     pModePred = []
 34     % bank of Kalman filters
 35     % (Cell array with <s.maxMode> elements of ClassKF)
 36     kf = {}
 37     % number of modes
 38     % (positive integer)
 39     maxMode = 0
 40     % weighting factors for mixed mode estimations
 41     % (matrix of dimension <s.maxMode> x <s.maxMode>) with sum(row) = 1
 42     mixWei = []
 43     % matrices to hold debug information
 44     oldxMeanOfNewMode = []
 45     oldErrCovOfNewMode = []
 46     % dimension of estimatee state; (positive integer)
 47     xDim = 0;
 48     % dimension of output; (positive integer)
 49     yDim = 0;
 50     % dimension of input matricx for every mode; 
 51     % (array of dimension <s.maxMode> with positive integers)
 52     uDim = [];    
 53     % estimated mean value; (vector of dimension <xDim> and type float) 
 54     xMean = [];
 55     % error covariance matrix;
 56     % (matrix of dimension <xDim> x <xDim> of floats)
 57     xErrCov = [];
 58     % struct to store recovery information of the actuatl state of the IMM;
 59     % (struct with fields <pMode>, <xMean> and <xErrCov> which are current 
 60     % time step copies of the respective class properties)
 61     immState = {}
 62   end
 63   
 64   properties(Constant)
 65     % lower bound for mode probabilies
 66     lowestProbVal = 0.000000000001,
 67   end
 68     
 69 
 70   methods
 71     function s = ClassIMM(A, B, C, wCov, vCov, x0Mean, x0ErrCov, trans)
 72     % ClassIMM: Creates object of a interacting multiple model filter (IMM)
 73     %
 74     % Synopsis:
 75     % [s] = ClassIMM(A, B, C, wCov, vCov, x0Mean, x0ErrCov, trans)
 76     %
 77     % Input Paramter:
 78     % - A: 1. one state matrix of estimatee valid for all modes;
 79     %      (matrix of dimension <s.xDim> x <s.xDim>)
 80     %       or
 81     %      2. cell array of state matrices specifying the state matrix of 
 82     %      each mode seperately;
 83     %      (cell array of dimension (1 x <s.maxMode>) with elements are
 84     %      matrices of dimension <s.xDim> x <s.xDim>)
 85     % - B: 1. one input matrix of estimatee valid for all modes;
 86     %      (matrix of dimension <s.xDim> x <s.uDim>)
 87     %       or
 88     %      2. cell array of input matrices specifying the input matrix of 
 89     %      each mode seperately;
 90     %      (cell array of dimension (1 x <s.maxMode>) with elements are
 91     %      matrices of dimension <s.xDim> x <s.uDim>)
 92     % - C: 1. one output matrix of estimatee valid for all modes;
 93     %      (matrix of dimension <s.yDim> x <s.xDim>)
 94     %        or
 95     %      2. cell array of output matrices specifying output matrix of 
 96     %      each mode seperately;
 97     %      (cell array of dimension (1 x <s.maxMode>) with elements are
 98     %      matrices of dimension <s.yDim> x <s.xDim>)
 99     % - wCov: 1. one process noise covariance matrix valid for all modes;
100     %         (matrix of dimension <s.xDim> x <s.xDim>)
101     %           or
102     %         2. cell array of covariance matrices specifying the process
103     %         noise covariance matrix seperately for each mode
104     %         cell array of dimension (1 x <s.maxMode>) with elements are
105     %         matrices of dimension <s.xDim> x <s.xDim>)
106     % - vCov: 1. one measurment noise covariance matrix valid for all modes
107     %         (matrix of dimension <s.yDim> x <s.yDim>)
108     %           or
109     %         2. cell array of covariance matrices specifying the 
110     %         measurement noise covariance matrix seperately for each mode
111     %         (cell array of dimension (1 x <s.maxMode>) with elements are
112     %         matrices of dimension <s.xDim> x <s.xDim>)
113     % - x0Mean: initial value of expected mean value; 
114     %           (vector of dimension <xDim> and type float) 
115     % - x0ErrCov: initial value of error covariance matrix;
116     %             (matrix of dimension <xDim> * <xDim>)
117     % - trans: transition matrix of the Markov chain that governs the 
118     %          switching of the different system models
119     %          (matrix with dimension <s.maxMode> x <s.maxMode>)
120     %
121     % Output Paramter:
122     % - s: created IMM object; (ClassIMM)       
123 
124       % ------- Check input parameter
125       % check trans
126       s.maxMode = size(trans, 1);
127       if( size(trans, 1) ~= size(trans, 2) )
128         trans %#ok<NOPRT>
129         error('** Input parameter trans is not a square matrix **')
130       elseif( sum(trans, 2) ~= ones(size(trans, 1), 1))
131         trans %#ok<NOPRT>
132         error(['** Input parameter trans is not a transition matrix ' ...
133           '(rows have to sum up to one) **']);
134       elseif( ((trans < 0 ) | (trans > 1)) ~= zeros(s.maxMode) )
135         trans %#ok<NOPRT>
136         error(['** Input parameter <trans> is not a transition matrix ' ...
137           '(elements must be 0, 1 or inbetween) **']);
138       end
139       s.trans = trans;
140 
141       % system matrix: A
142       if( iscell(A) )
143         if( ~isequal(size(A), [1, s.maxMode]) )
144           size(A)
145           error(['** Wrong dimension of input parameter A. ' ...
146             'If A is a cell array then size(A) must be ' ...
147             'equal to [1, <s.maxMode>] = [1, size(trans,1)] = '...
148             '[1, %d] **'], s.maxMode)
149         end
150         % all matrices have to be of same size
151         [d1, d2] = cellfun(@size, A);
152         if( ~isequal([d1', d2'], repmat(size(A{1}), s.maxMode, 1)) )
153           error(['** Wrong dimension of input parameter A. ' ...
154           'All elements of A must be square matrices **'])
155         end
156         s.xDim = size(A{1}, 1);
157         rrA = A;
158       else
159         if( size(A, 1) ~= size(A, 2) ) 
160           size(A)
161           error(['** Wrong dimension of input parameter A. ' ...
162             'A must be a square matrix']);
163         end
164         s.xDim = size(A, 1);
165         for i = 1 : s.maxMode; rrA{i} = A; end
166       end
167       
168       % input matrix: B
169       if( iscell(B) )
170         if ( ~isequal(size(B), [1, s.maxMode]) )
171           size(B)
172           error(['** Wrong dimension of input parameter B. ' ...
173             'If B is a cell array then size(B) must be ' ...
174             'equal to [1, <s.maxMode>] = [1, size(trans, 1)] = ' ...
175             '[1, %d] **'], s.maxMode)
176         end
177         [d1, d2] = cellfun(@size, B);
178         if( (~all(d1 == (s.maxMode * s.xDim))) || ...
179             (~all(d2 == size(B{1}, 2))) )
180           size(B)
181           error(['** Wrong dimension of input parameter B. ' ...
182             'Every matrix in cell B must have same dimension and' ...
183             ' size(B{i}, 1) must be <s.xDim> = %d', s.xDim])
184         end
185         s.uDim = d2(1);
186         rrB = B;
187       else
188         if( size(B, 1) ~= s.xDim )
189           size(B)
190           error(['** Wrong dimension of input parameter B. ' ...
191             'It must hold: size(B, 1) = <s.xDim> = %d'], s.xDim);
192         end
193         s.uDim = size(B, 2);
194         for i = 1 : s.maxMode; rrB{i} = B; end
195       end
196       
197       % output matrix: C
198       if( iscell(C) )
199         if( ~isequal(size(C), [1, s.maxMode]) )
200           size(C)
201           error(['** Wrong dimension of input parameter C. ' ...
202             'If C is a cell array then size(C) must be ' ...
203             'equal to [1, <s.maxMode>] = [1, size(trans, 1)] = '...
204             '[1, %d] **'], s.maxMode)
205         end
206         [~, d2] = cellfun(@size, C);
207         if( d2 ~= (ones(1, s.maxMode) * s.xDim) )
208           size(C)
209           error(['** Wrong dimension of input parameter C. ' ...
210             'It must hold for every matrix in cell C: '...
211             'size(C, 2) = <s.xDim> = %d **'], s.xDim)
212         end
213         s.yDim = size(C{1}, 1);
214         rrC = C;
215       else
216         if( size(C, 2) ~= s.xDim )
217           size(C)
218           error(['** Wrong dimension of input parameter C. ' ...
219             'It must hold: size(C, 2) = <s.xDim> = %d'], s.xDim);
220         end
221         s.yDim = size(C, 1);
222         for i = 1 : s.maxMode; rrC{i} = C; end
223       end
224       
225       % process noise: wCov
226       if( iscell(wCov) )
227         if ( ~isequal(size(wCov), [1, s.maxMode]) )
228           wCov %#ok<NOPRT>
229           error(['** Wrong dimension of input parameter wCov. ' ...
230             'If wCov is a cell array then size(wCov) must be ' ...
231             'equal to [1, size(trans, 1)] = [1, %d] **'], s.maxMode)
232         end
233         [d1, d2] = cellfun(@size, wCov);
234         if( ~isequal([d1; d2], (ones(2, s.maxMode) * s.xDim) ) )
235           wCov %#ok<NOPRT>
236           error(['** Wrong dimension of input parameter wCov. ' ...
237             'It must hold for every matrix in cell wCov: '...
238             'size(wCov) = <s.xDim>^2 = [%d, %d] **'], s.xDim)
239         end
240         rrWcov = wCov;
241       else
242         % wCov is not a cell array
243         if( ~isequal(size(wCov), [s.xDim, s.xDim]) )
244           size(wCov)
245           error(['** Wrong dimension of input parameter wCov. ' ...
246             'It must hold: size(wCov) = <s.xDim>^2 = [%d, %d]'], ...
247             s.xDim, s.xDim);
248         end
249         for i = 1 : s.maxMode
250           rrWcov{i} = wCov;
251         end
252       end
253       
254       % measurement noise: vCov
255       if( iscell(vCov) )
256         if( ~isequal(size(vCov), [1, s.maxMode]))
257           vCov %#ok<NOPRT>
258           error(['** Wrong dimension of input parameter vCov. ' ...
259             'If vCov is a cell array then size(vCov) must be ' ...
260             'equal to [1, <s.maxMode>] = [1, %d] **'], s.maxMode)
261         end
262         [d1, d2] = cellfun(@size, vCov);
263         if( ~isequal([d1; d2], (ones(2, s.maxMode) * s.yDim)) )
264           vCov %#ok<NOPRT>
265           error(['** Wrong dimension of input parameter vCov. ' ...
266             'It must hold for every matrix in cell vCov: '...
267             'size(vCov) = <s.yDim>^2 = [%d, %d] **'], s.yDim)
268         end
269         rrVcov = vCov;
270       else
271         % vCov is not a cell array
272         if( ~isequal(size(vCov), [s.yDim, s.yDim]) )
273           size(vCov)
274           error(['** Wrong dimension of input parameter vCov. ' ...
275             'It must hold: size(vCov) = <s.xDim>^2 = [%d, %d]'], ...
276             s.yDim, s.yDim);
277         end
278         for i = 1 : s.maxMode; rrVcov{i} = vCov; end
279       end
280       
281       % check initial state values
282       if( ~isequal(size(x0Mean), [s.xDim, 1]) )
283         x0Mean %#ok<NOPRT>
284         error('** Wrong parameter dimension x0Mean **')
285       end
286       if( ~isequal(size(x0ErrCov), [s.xDim, s.xDim]) )
287         x0ErrCov %#ok<NOPRT>
288         error('** Wrong parameter dimension x0ErrCov **')
289       end
290    
291       % init old transition probabilities; it is only important that first 
292       % mode is 0 and sum = 1
293       s.pMode = [zeros(s.maxMode - 1, 1); 1];
294       % init weighting factors for mixing the mode estimations
295       s.mixWei = zeros(s.maxMode);
296       % init stacked, mixed estimates
297       s.oldxMeanOfNewMode = zeros(s.xDim, s.maxMode);
298       % init stacked, mixed error covariance matrix
299       s.oldErrCovOfNewMode = zeros(s.xDim, s.xDim, s.maxMode);
300 
301       % init elemtary kalman filters (one per mode) and init struct for 
302       % state recovery (used for time delayed implementation)
303       for mode = 1 : 1 : s.maxMode
304         s.kf{mode} = ClassKF(rrA{mode}, rrB{mode}, rrC{mode}, ...
305           rrWcov{mode}, rrVcov{mode}, x0Mean, x0ErrCov);
306         s.immState{mode, 1} = struct('pMode', s.pMode(mode), ...
307           'xMean', s.kf{mode}.xMean, 'xErrCov', s.kf{mode}.xErrCov);
308       end
309     end
310     
311     function xMean = Execute(s, uInput, y)
312     % Execute: Function executes one time step of the IMM filter.
313     % Inputs of the function are a vector with the control inputs by every
314     % model and one measurement (if available). If there is no measurment
315     % available, only a prediction is performed. Output is the overall 
316     % estimated state of the considered multiple model system.
317     %
318     % Synopsis: 
319     % xMean = Execute(s, uInput)
320     % xMean = Execute(s, uInput, y)
321     %
322     % Input Paramter:
323     % - s: object of the IMM-filter that should be executed;
324     %   (implicit); (ClassIMM)
325     % - uInput: control inputs of the system whereby uInput can be 1) a 
326     %   single control input that is applied to all models or 2) a matrix
327     %   which specifies the control inputs separately for each mode. The
328     %   control input for the i-th mode is in column i.
329     %   (1. vector of dimension <s.uDim>  or 2. matrix of dimension 
330     %   <s.uDim> x <s.maxMode>)
331     % - y: Measurement of the system state. If there is no measurement 
332     %   available then it must contain only "NaN" elements
333     %   and only a prediction is done;
334     %   (vector of dimension <s.yDim> of type float) or 
335     %   (vector of arbitrary size with only NaN elements)
336     %
337     % Output Parameter:
338     % - xMean: estimate of the state
339     %   (vector of dimension <s.xDim>)
340     %
341     % CONDITION: The function has to be executed once every time step
342 
343       %% ---- 0) check Input parameter
344       % uInput
345       if( size(uInput, 1) ~= s.uDim )
346         uInput %#ok<NOPRT>
347         error(['** Wrong dimension of input parameter uInput: size' ...
348           '(uInput, 1) is not equal to <s.uDim> = %d **'], s.uDim)
349       end
350       if( size(uInput, 2) == 1 )
351         % same control input for all models
352         uSeqMatrix = repmat(uInput, 1, s.maxMode );
353       elseif( size(uInput, 2) == s.maxMode )
354         % separate inputs; control inputs given separately for each model
355         uSeqMatrix = uInput;
356       else
357         uInput %#ok<NOPRT>
358         error(['** Wrong dimension of input parameter uInput: size' ...
359           '(uInput, 2) must be 1 or <s.maxMode> = %d **'], s.maxMode)
360       end
361       if( any(isnan(uInput)) )
362         uInput %#ok<NOPRT>
363         error('** Input parameter uInput has empty elements **')
364       end
365       
366       % y
367       if all(~isnan(y))
368         measurementReceived = true;
369       elseif( all(isnan(y)) && (isequal(size(y), size(y), [s.yDim, 1])) )
370         measurementReceived = false;
371       else
372         y %#ok<NOPRT>
373         error(['** Input paramter y must be a vector of non-NaN ' ...
374           'elements of dimension [<s.maxMode>, 1] = [%d, 1] OR a ' ... 
375           'NaN-scalar '], s.yDim)
376       end
377 
378       %% ---- 1) model-conditioned reinitialization for all modes
379       % 1a) predicted mode probability
380       s.pModePred(1:s.maxMode) = 0;
381       for newMode = 1 : 1 : s.maxMode
382         s.pModePred(newMode) = s.pMode' * s.trans(:, newMode);
383       end
384 
385       % 1b) mixing weights
386       for newMode = 1 : 1 : s.maxMode
387         for oldMode = 1 : 1 : s.maxMode
388           s.mixWei(oldMode, newMode) = s.trans(oldMode, newMode) *...
389             s.pMode(oldMode) / s.pModePred(newMode);
390         end
391       end
392 
393       % 1c) mixing estimates
394       for newMode = 1:1:s.maxMode
395         help = zeros(s.xDim, 1);
396         for oldMode = 1:1:s.maxMode
397             help = help + s.kf{oldMode}.xMean * s.mixWei(oldMode, newMode);
398         end
399         s.oldxMeanOfNewMode(:, newMode) = help;
400       end
401 
402       % 1d) mixing covariances
403       for newMode = 1:1:s.maxMode
404         help = zeros(s.xDim, s.xDim);
405         for oldMode = 1:1:s.maxMode
406           help = help + (s.kf{oldMode}.xErrCov + ...
407             (s.oldxMeanOfNewMode(:, newMode) - s.kf{oldMode}.xMean) *...
408             (s.oldxMeanOfNewMode(:, newMode) - s.kf{oldMode}.xMean)') *...
409             s.mixWei(oldMode, newMode);
410         end
411         s.oldErrCovOfNewMode(:, :, newMode) = help;
412       end
413 
414       % 1e) reinitializing Filters
415       for mode = 1:s.maxMode
416         s.kf{mode}.ReInit(s.oldxMeanOfNewMode(:, mode), ...
417           s.oldErrCovOfNewMode(:, :, mode));
418       end
419 
420       %% 2) ------- Kalman filtering for each mode
421       for mode = 1 : 1 : s.maxMode
422         s.kf{mode}.Execute(uSeqMatrix(:, mode), y);
423       end
424 
425       %% 3) --------- Mode probability update
426       % only perform update if measurement has been recieved
427       if measurementReceived == false
428         % no measurement -> mode probability equal to prediction      
429         s.pMode = s.pModePred';
430       else
431         % a)calculate model likelihoods
432         likHood = zeros(1, s.maxMode);
433         for mode = 1 : s.maxMode
434           likHood(mode) = exp(-s.kf{mode}.yRes' * ...
435             (s.kf{mode}.covRes \ s.kf{mode}.yRes) / 2) / ...
436             sqrt(2 * pi * det(s.kf{mode}.covRes));
437         end
438 
439         % b) calculate model probabilities
440         normMe = s.pModePred * likHood';
441         for mode = 1:s.maxMode 
442           s.pMode(mode) = s.pModePred(mode) * likHood(mode) / normMe;
443         end
444       end
445       % c) bounding to avoid non-singularity issues
446       renorm = false;
447       for mode = 1:s.maxMode
448         if( s.pMode(mode) < ClassIMM.lowestProbVal )
449           s.pMode(mode) = ClassIMM.lowestProbVal;
450           renorm = true;
451         end
452       end
453       if( renorm == true )
454         s.pMode = s.pMode / sum(s.pMode);
455       end
456 
457       %% 4) ---------- Estimate fusion and set output
458       helpMean = 0;
459       for mode = 1:s.maxMode
460         helpMean = helpMean + s.pMode(mode) * s.kf{mode}.xMean;
461       end
462       s.xMean = helpMean;
463 
464       helpCov = zeros(s.xDim, s.xDim);
465       for mode = 1:s.maxMode
466         helpCov = helpCov + (s.kf{mode}.xErrCov + ...
467           (s.xMean - s.kf{mode}.xMean) * ...
468           (s.xMean - s.kf{mode}.xMean)') * s.pMode(mode);
469       end
470       s.xErrCov = helpCov;
471 
472       % setoutput
473       xMean = s.xMean;
474 
475       %% 5) ------------ Update current Imm State
476       for mode = 1:s.maxMode
477         s.immState{mode}.pMode = s.pMode(mode);
478         s.immState{mode}.xMean = s.kf{mode}.xMean;
479         s.immState{mode}.xErrCov = s.kf{mode}.xErrCov;
480       end
481       
482     end
483   
484     
485     function ReInit(s, pastImmState)
486     % ReInit: Function reinitializes the IMM. This means that the expected 
487     % mean values and the error covariance matrices of the elementary 
488     % Kalman filters are reinitialized as well as the mode probabilities
489     % of the IMM.
490     %
491     % Synopsis:
492     % ReInit(s, pastImmState)
493     %
494     % Input Paramter:
495     % - s: IMM object to be re-initialized; 
496     %   (implicit); (ClassIMM)
497     % - pastImmState: recovery information of all the IMM state to be 
498     %   recovered;
499     %   (cellarray of dimension [<s.maxMode>, 1] with elements are structs 
500     %   with fields <pMode>, <xMean> and <xErrCov>; dimensions
501     %   of these fields are equal to the corresponding class property)
502     %
503     % Output Paramter:
504     % - none
505       % check input
506       if( ~iscell(pastImmState) || ...
507           ~isequal(size(pastImmState), [s.maxMode, 1]) )
508           pastImmState  %#ok<NOPRT>
509           error('** Wrong format of input Parameter pastImmState **')
510       end
511       for mode = 1:s.maxMode
512         s.pMode(mode) = pastImmState{mode, 1}.pMode;
513         s.kf{mode}.ReInit(pastImmState{mode, 1}.xMean , ...
514           pastImmState{mode, 1}.xErrCov);
515       end
516     end
517   end
518 end
519 
520 %% only for debug: code to get transistion matrix for unity distribution
521 % for rowCounter = 1 : 1 : s.maxMode
522 %   for colCounter = 1 : 1 : s.maxMode
523 %     if colCounter < (rowCounter + 1)
524 %       s.trans(rowCounter, colCounter) = 1 / s.maxMode;
525 %     elseif colCounter == (rowCounter + 1) 
526 %       s.trans(rowCounter, colCounter) = 1 - rowCounter / s.maxMode;
527 %     else
528 %       s.trans(rowCounter, colCounter) = 0;
529 %     end
530 %       end
531 %   end
  1 classdef ClassKF < handle
  2   % ClassKF: This class represents a kalman-filter. The kalman-filter
  3   % calculates the linear minimum mean square state estimate of a system
  4   % based on a model of that system, the control input applied to the 
  5   % system and, if available, a measurement of the state.
  6   %
  7   % CONDITION: The function Execute has to be executed once every time 
  8   % step.
  9 
 10   properties(SetAccess = private)
 11     % estimated mean value; (vector of dimension <xDim> and type float) 
 12     xMean = [];
 13     % error covariance matrix;
 14     % (matrix of dimension <xDim> * <xDim> of floats)
 15     xErrCov = [];
 16     % state matrix of estimatee;
 17     % (matrix of dimension <xDim> * <xDim> of floats)
 18     A = [];
 19     % input matrix of estimatee;
 20     % (matrix of dimension <uDim> * <uDim> of floats) 
 21     B = [];
 22     % output matrix of estimatee;
 23     % (matrix of dimension <uDim> * <uDim> of floats)
 24     C = [];
 25     % dimension of estimatee state; (positive integer)
 26     xDim = 0;
 27     % dimension of output; (positive integer)
 28     yDim = 0;
 29     % dimension of input; (positive integer)
 30     uDim = 0;
 31     % covariance matrix of measurement noise; 
 32     % (matrix of dimension <yDim> * <yDim> of floats)
 33     vCov = [];
 34     % covariance matrix of process noise; 
 35     % (matrix of dimension <xDim> * <xDim> and of floats)
 36     wCov = [];
 37     % residuum of covariance matrix; 
 38     % (matrix of dimension <xDim> * <xDim> of floats)
 39     covRes = 0;
 40     % measuremt residuum; (vector of dimension <yDim> and type float)
 41     yRes = 0;
 42   end
 43 
 44   properties(Constant)
 45     % number of decimals that are considered in the function isCovMatrix
 46     % to check for symmetry of the matrix
 47     % <positive integer>
 48     dec = 20;
 49   end
 50   methods
 51     function s = ClassKF(A, B, C, wCov, vCov, xMean0, xErrCov0)
 52     % ClassKF: Creates an kalman Filter object
 53     %
 54     % Synopsis:
 55     % [s] = ClassKF(A, B, C, wCov, vCov, xMean0, xErrCov0)
 56     %
 57     % Input Paramter:
 58     % - A: state matrix of estimatee; (matrix of dimension <xDim> * <xDim>) 
 59     % - B: input matrix of estimatee; (matrix of dimension <uDim> * <uDim>)
 60     % - C: output matrix of estimatee;(matrix of dimension <uDim> * <uDim>) 
 61     % - wCov: covariance matrix of process noise; 
 62     %   (matrix of dimension <xDim> * <xDim> and of floats)
 63     % - vCov: covariance matrix of process noise; 
 64     %   (matrix of dimension <xDim> * <xDim> and of floats) 
 65     % - xMean0: initial value of expected mean value; 
 66     %   (vector of dimension <xDim> and type float) 
 67     % - xErrCov0: initial value of error covariance matrix;
 68     %   (matrix of dimension <xDim> * <xDim>)
 69     %
 70     % Output Paramter:
 71     % - s: Kalman filter object; (ClassKF)
 72 
 73       % init system model
 74       [d1 d2] = size(A);
 75       if(d1 ~= d2)
 76         disp(A)
 77         error('** Wrong dimension of input paramter A **')
 78       else
 79         s.A = A;
 80         s.xDim = d1;
 81       end
 82       [d1 d2] = size(B);
 83       if (d1 ~= s.xDim)
 84         disp(A)
 85         disp(B)
 86         error('** Dimension mismatch matrix A and B **')
 87       else
 88         s.B = B;
 89         s.uDim = d2;
 90       end
 91       [d1 d2] = size(C);
 92       if (d2 ~= s.xDim)
 93         disp(A)
 94         disp(C)
 95         error('** Dimension mismatch matrix A and C **')
 96       else
 97         s.C = C;
 98         s.yDim = d1;
 99       end
100       
101       % init process noise covariances
102       if( ~ClassKF.isCovMatrix(wCov, s.xDim) )
103         disp(s.xDim)
104         disp(wCov)
105         error('** Input parameter wCov is not a covariance matrix. **')
106       end
107       s.wCov = wCov;
108      
109       % init measurement noise covariance
110       if( ~ClassKF.isCovMatrix(vCov, s.yDim) )
111         disp(vCov)
112         error('** Input parameter vCov is not a covariance matrix. **')
113       end
114       s.vCov = vCov;
115 
116       % init initial state values
117       s.ReInit(xMean0 , xErrCov0);
118     end
119     
120     function xMean = Execute(s, u, varargin)
121     % Execute: Function executes one time step of the Kalman filter.
122     % Input is the applied control input and a measurement (if available).
123     % If there is no measurement, only the prediction step will be
124     % performed.
125     %
126     % Synopsis: 
127     % Execute(s, u)
128     % Execute(s, u, y)
129     %
130     % Input Paramter:
131     % - s: object of kalman-filter to be executed; 
132     %   (implicit); (ClassKF)
133     % - u: control input of the system; 
134     %   (column vector of dimension <s.uDim> of type float)  
135     % - y: (optional) Measurement of the state of the system; if no 
136     %   measurement is available the measurement hat to be set to NaN'
137     %   (column vector of dimension <s.yDim> of type float) or 
138     %   (Vector of arbitry with all elements NaN)
139     %
140     % Output Parameter:
141     % - xMean: estimated state of the system
142     %          (vector of dimension <s.xDim> x <s.xDim>
143     %
144     % CONDITION: The function has to be executed once every time step
145 
146       % ----------------- 1) Time update
147       % a) check Input
148       if( ~isequal( size(u), [s.uDim, 1]) )
149         disp(u)
150         error('** Wrong dimension of input parameter u **')
151       end
152       if( all(isnan(u)) )
153         disp(u)
154         error('** Input parameter u has empty elements **')
155       end
156       % b) prediction
157       s.xMean = s.A * s.xMean + s.B * u;
158       s.xErrCov = s.A * s.xErrCov * s.A' +  s.wCov;     
159       
160       % -------------- 2) Measurement update
161       % a) check Input
162       if nargin > 3
163         error(['** Execute: %d input parameters - ', ...
164             'only 1 or 2 expected.'], nargin - 1)
165       end
166       if nargin == 3
167         y = varargin{1};
168         if( all(isnan(y)) )
169           % no measurement -> no measurement update
170         else
171           if( ~isequal(size(y), [s.yDim, 1]) )
172             disp(y)
173             error('** Wrong dimension of input parameter y **')
174           end
175           if( any(isnan(y)))
176             disp(y)
177             error('** Input paramter y has NaN elements **')
178           end
179           % b) filter algorithm
180 %           s.covRes = s.C * s.xErrCov * s.C' + s.vCov;
181 %           K = s.xErrCov * s.C' / (s.covRes);
182 %           s.yRes = y - s.C * s.xMean;        
183 %           s.xMean = s.xMean + K * s.yRes;
184 %           s.xErrCov = s.xErrCov - K * s.covRes * K';
185             
186             % information form
187             s.covRes = s.C * s.xErrCov * s.C' + s.vCov;
188             s.yRes = y - s.C * s.xMean;        
189             xErrCovPost = inv(inv(s.xErrCov) + s.C' * inv(s.vCov) * s.C);
190             s.xMean = xErrCovPost * (s.xErrCov \ s.xMean) + ...
191               xErrCovPost * s.C' * (s.vCov \ y);
192             s.xErrCov = xErrCovPost;
193         end
194       else
195         % no measurement --> no measurement update
196       end
197       % set output
198       xMean = s.xMean;
199     end
200 
201     
202     function ReInit(s, xMean0 , xErrCov0)
203     % ReInit: Function reinitializes the expected mean value and the
204     % error covariance matrix of the Kalman filter
205     %
206     % Synopsis:
207     % ReInit(s, xMean0 , xErrCov0)
208     %
209     % Input Paramter:
210     % - s: Kalman-filter to be re-initialized; 
211     %   (implicit); (ClassKF)
212     % - xMean0: new value of the mean value; 
213     %   (column vector of dimension <xDim> and type float)  
214     % - xErrCov0: new error covariance matrix; 
215     %   (matrix of dimension <xDim> * <xDim>) 
216     %
217     % Output Paramter: none
218 
219       % init initial state values
220       if( isequal(size(xMean0), [s.xDim, 1]) )
221         s.xMean = xMean0;
222       else
223       disp(xMean0)
224         error('** Dimension of xMean0 is not equal to [%d, 1] **', s.xDim)
225       end
226       if( ClassKF.isCovMatrix(xErrCov0, s.xDim) )
227         s.xErrCov = xErrCov0;
228       else
229         disp(xErrCov0)
230         error(['** xErrCov is not a covariance matrix or not of ' ...
231           'dimension [%d, %d] **'], s.xDim, s.xDim);
232       end
233     end
234   end
235   
236   methods(Static)
237     % isCovMatrix: Function tests wether a matrix can be a covariance
238     % matrix or not.
239     %
240     % Synopsis:
241     % out = isCovMatrix(matIn, dim)
242     %
243     % Input Paramter:
244     % - matIn: 2-dimensional matrix to be tested
245     % - dim: dimension the covariance should have; 
246     %   <positive integer>
247     %
248     % Output Paramter:
249     % - out: true if matrix is covariance of dimension <dim> x <dim>
250     %        false if not
251     function out = isCovMatrix(matIn, dim)
252       % tests wether matrix is squqre AND
253       % symmetric AND
254       % diagonal elements are not negative
255       % Factor dec used to round decimals. Else symmetry condition could
256       % return a false negative
257       out = false;
258       if isequal(size(matIn), [dim, dim])
259         % rounding
260         matIn = round(matIn .* ClassKF.dec) ./ ClassKF.dec;
261         if( all(diag(matIn) >= 0) )
262         %if(all(all(isequal(round(tril(matIn) .* ClassKF.dec)./ ClassKF.dec,
263         %(round(triu(matIn) .* ClassKF.dec)./ ClassKF.dec) .')))) & 
264           out = true;
265         end
266       end
267     end
268   end
269 end
  1 function CloudRunnerMain(simRuns, simCycles, setPointReverseSteps, ...
  2   uDelProbs, yDelProbs)
  3 % This function provides a simulation framework to simulate an inverted 
  4 % pendulum that is controlled and sensored via a communication network
  5 % that is subject to stochastic data losses and stochastic time delays.
  6 % Several estimators are implemented to estimate the state of the
  7 % inverted pendulum and these estimators are compred by means of their
  8 % RMSE.
  9 %   The control loop consists 1. of an sequence-based LQR with adjustable
 10 % setpoint, 2. a UDP-like controller-actuator communication network, 3. an
 11 % actuator, 4. a inearized inverted pendulum on a cart, 5. a sensor which
 12 % takes noisy measurements of the cart position and the angle of the
 13 % pendulum, 6. a sensor-estimator communication network and 7. several
 14 % estimators that should be compared. 
 15 %   The estimators, that are compared, as well as the setup are described
 16 % in detail in the paper: 
 17 % Joerg Fischer, Achim Hekler, Uwe D. Hanebeck,
 18 % State Estimation in Packet-Based Networked Control Systems,
 19 % Proceedings of the 15th International Conference on Information Fusion
 20 % (Fusion 2012) (to appear), Singapore, July, 2012. 
 21 % For further details regarding the components of the control loop please
 22 % refer to the corresponding Class files. 
 23 %
 24 % Synopsis:
 25 % CloudRunnerMain(simRuns, simCycles, setPointReverseSteps,...
 26 %    uDelProbs, yDelProbs)
 27 %
 28 % Input Parameter:
 29 % - simRuns: number of total Monte Carlo simulation runs. One run consists
 30 %            of <simCycles> number of time steps, where the length of one
 31 %            time step is defined by the sample time. The simulation
 32 %            parameters are given as data sets and can change betweed two
 33 %            simulation run. After all simulation  runs the RMSE is build
 34 %            by averaging all RMSE of a specific time step for each filter;
 35 %            <positive integer>
 36 % - simCycles: number of time steps simulated per simulation run; 
 37 %              <positive integer>
 38 % - setPointReverseSteps: number of time steps after which the setpoint of
 39 %                         the pendulum is reversed, e.g., from [3, 0] to
 40 %                         [-3, 0]; 
 41 %                         <positive integer>
 42 % - uDelProbs: time delay probability distribution of the CONTROLLER-
 43 %              ACTUATOR-NETWORK. Thereby the i-th entry of this vector is 
 44 %              the probability that a delay of i time steps occurs;
 45 %              the last entry of this vector gives the data loss rate of
 46 %              the of the network (if vector has three or more entries)
 47 %              Sum of this vector must be equal to 1;
 48 %              <vector of positive floats>
 49 % - yDelProbs: time delay probability distribution of the SENSOR-
 50 %              ESTIMATOR-NETWORK. Thereby the i-th entry of this vector is 
 51 %              the probability that a delay of i time steps occurs;
 52 %              the last entry of this vector gives the data loss rate of
 53 %              the of the network (if vector has three or more entries)
 54 %              Sum of this vector must be equal to 1;
 55 %              <vector of positive floats>
 56 %
 57 % Output Parameter:
 58 % - None
 59 %
 60 % Output Plots:
 61 % - Plot1: sample trajectory of all four states of the inverted pendululm
 62 %          (cart position, cart velocity, pendulum angel, pendulum
 63 %          velocity) for real state, LMMSE filter (sKF) and proposed
 64 %          estimator (IMM)
 65 % - Plot2: measurements of the two observable states (cart position,
 66 %          penulum angle) 
 67 % - Plot3: control input applied by the actuator
 68 % - Plot4: RMMSE of all 4 states for LMMSE filter (sKF) and proposed
 69 %          estimator (IMM) 
 70 %
 71 % See also: ClassActuator.m, ClassCommNetwork.m, ClassDataPacket.m,
 72 %           ClassActuator.m, calcDelayTrans.m, ClassDelayedKF.m, ClassKF.m,
 73 %           ClassDelayedIMM.m, ClassIMM
 74 
 75 
 76   % external copied functions, please put them here
 77   path(path,'extern')
 78   
 79   %% ------------------------------------------------------------
 80   % --------- 0) Check Inputs  -----------------------------------
 81   % --------------------------------------------------------------
 82   %
 83   % Value considered for rounding error tolerance
 84   roundingErr = 0.00000001;
 85   % uDelProbs
 86   if size(uDelProbs, 1) ~= 1
 87     uDelProbs %#ok<NOPRT>
 88     error('** Input parameter uDelProbs is no colum vector **')
 89   else
 90     if min(uDelProbs) < 0
 91       uDelProbs %#ok<NOPRT>
 92       error('** Input parameter uDelProbs has negative entries **')
 93     end
 94     if (round(sum(uDelProbs ./ roundingErr)) * roundingErr) ~= 1
 95       uDelProbs %#ok<NOPRT>
 96       error(['** Sum of entries of input paramter uDelProbs is' ...
 97         ' not equal to 1 **'])
 98     end
 99   end
100   % yDelProbs
101   if size(yDelProbs, 1) ~= 1
102     yDelProbs %#ok<NOPRT>
103     error('** Input parameter yDelProbs is no colum vector **')
104   else
105     if min(yDelProbs) < 0
106       yDelProbs %#ok<NOPRT>
107       error('** Input parameter yDelProbs has negative entries **')
108     end
109     if (round(sum(yDelProbs ./ roundingErr)) * roundingErr) ~= 1
110       yDelProbs %#ok<NOPRT>
111       error(['** Sum of entries of input paramter yDelProbs is' ...
112         ' not equal to 1 **'])
113     end
114   end
115   % simCycles
116   if ~isscalar(simCycles)
117     simCycles %#ok<NOPRT>
118     error('** Input parameter simCycles not a scalar **')
119   elseif(simCycles < 0) || ((simCycles - floor(simCycles)) ~= 0)
120     simCycles %#ok<NOPRT>
121     error('** Input parameter simCycles is negative **')
122   end    
123   % simRuns
124   if ~isscalar(simRuns)
125     simRuns %#ok<NOPRT>
126     error('** Input parameter simRuns not a scalar **')
127   elseif(simRuns < 0) || ((simRuns - floor(simRuns)) ~= 0)
128     simRuns %#ok<NOPRT>
129     error('** Input parameter simRuns is negative **')
130   end    
131   % setPointReverseSteps
132   if ~isscalar(setPointReverseSteps)
133     setPointReverseSteps %#ok<NOPRT>
134     error('** Input parameter setPointReverseSteps not a scalar **')
135   elseif(setPointReverseSteps < 0) || ...
136       ((setPointReverseSteps - floor(setPointReverseSteps)) ~= 0)
137     setPointReverseSteps %#ok<NOPRT>
138     error('** Input parameter setPointReverseSteps is negative **')
139   end
140 
141 
142   %% ------------------------------------------------------------
143   % --------- 1) Set Main Simulation Parameters -----------------
144   % -------------------------------------------------------------
145   %
146   % set setpoint of pendulum;
147   % vector is of format [cart position; pendulum angle] in meters and
148   % degree, respectively
149   setpoint = [2;0];
150 
151   % --- 1b) Set network time delay probability distributions
152   %
153   % set time delay probability distribution of the CONTROLLER-ACTUATOR-
154   % NETWORK that is used to simulate the real system;
155   % the cell array uDelProbsNwMc gives the possibilities to use different
156   % distributions for different Monte Carlo simulation runs. Here, only the
157   % distribution given by the user is used
158   uDelProbsNwMc{1} = uDelProbs;
159   % set time delay probability distribution of the CONTROLLER-ACTUATOR-
160   % NETWORK that is considered by the estimator. If uDelProbsNwMc is
161   % different to uDelProbsFilter, this means that the filter uses a
162   % distribution that does not reflect the real network. Here, both
163   % distribution are identical
164   uDelProbsFilter  = uDelProbs;
165   % set time delay probability distribution of the SENSOR-ESTIMATOR-NETWORK
166   % that is used to simulate the real system;
167   % the cell array yDelProbsNwMc gives the possibilities to use different
168   % distributions for different Monte Carlo simulation runs. Here, only the
169   % distribution given by the user is used
170   yDelProbsNwMc{1} = yDelProbs;
171   % set time delay probability distribution of the SENSOR-ESTIMATOR-NETWORK
172   % that is considered by the estimator. If yDelProbsNwMc is
173   % different to yDelProbsFilter, this means that the filter uses a
174   % distribution that does not reflect the real network. Here, both
175   % distribution are identical.
176   yDelProbsFilter  = yDelProbs;
177 
178   % --- 1c) Init Inverted Pendulum on a cart. 
179   % This system has 4 states: 
180   % cart position, cart velocity, pendulum angel, pendulum angle velocity;
181   %
182   % mass of cart
183   M = 0.5;
184   % mass of pendulum
185   m = 0.5;
186   % friction of the cart
187   b = 0.1;
188   % inertia of the pendulum
189   i = 0.006;
190   g = 9.8;
191   % length to pendulum center of mass
192   l = 0.3;
193   % sample time
194   Ts = 1 / 100;
195   % initial state
196   xRealMeanMc = [0; 0.2; 0.2; 0];
197   % initial state covariance
198   xRealCovMc = 0.1 : 0.1 : 1;
199   % set of variances of process noise of the cart position;
200   % entries are in meters;
201   % for each run of the monte carlo simulation one entry is randomly chosen
202   wVarXMc = [0.01, 0.03, 0.06, 0.09, 0.12, 0.2].^2;
203   % set of variances of process noise of the pendulum angle;
204   % entries are in degrees;
205   % for each run of the monte carlo simulation one entry is randomly chosen
206   wVarPhiMc = ([0.1, 0.5, 1, 2, 3] * 2 * pi / 360).^2;
207   % set of variances of measurement noise of the cart position;
208   % entries are in meters;
209   % for each run of the monte carlo simulation one entry is randomly chosen
210   vVarXMc = [0.01, 0.03, 0.06, 0.09, 0.12, 0.2].^2;
211   % set of variances of measurement noise of the pendulum angle;
212   % entries are in meters;
213   % for each run of the monte carlo simulation one entry is randomly chosen
214   vVarPhiMc = ([0.1, 0.5, 1, 2, 3] * 2 * pi / 360).^2;
215   % calculate discrete-time system matrices of the inverted pendulum
216   p = i * (M + m) + M * m * l^2;
217   Ac = [0, 1, 0, 0;
218         0, -(i + m * l^2) * b / p, (m^2 * g * l^2) / p, 0;
219         0, 0, 0, 1;
220         0, -(m * l * b) / p, m * g * l * (M + m) / p,  0];
221   Bc = [ 0; (i + m * l^2) / p;...
222          0;...
223          m * l / p];
224   C = [1 0 0 0;...
225        0 0 1 0];
226   D = [0; 0];
227   sys = c2d(ss(Ac, Bc, C, D), Ts);
228   A = sys.a;
229   B = sys.b;
230   C = sys.c;
231 
232   % --- 1d) Init LQR-controller.
233   % This tries to stabilitze the pendulum and to steer it to the given 
234   % setpoint.
235   %
236   % LQR state weihting matrix
237   Q = [5000 0 0 0; 0 0 0 0; 0 0 100 0; 0 0 0 0];
238   % LQR control input weihting matrix
239   R = 100;
240   [L,~,~] = dlqr(A,B,Q,R);
241   % validity check if given setpoint has right dimension
242   if(~isequal(size(setpoint), [size(C, 1), 1]))
243     error('** Wrong dimension of setpoint **');
244   end
245 
246   % --- 1e) memory allocation for system variables
247   % 
248   % control input applied by the actuator
249   uReal = zeros(size(B, 2), simCycles + 1, simRuns);
250   % state of the plant (inverted pendulum)
251   xReal = zeros(size(A, 1), simCycles + 1, simRuns);
252   % output of the plant (inverted pendulum)
253   yReal = zeros(size(C, 1), simCycles + 1, simRuns);
254   % measurement (output of the plant pertubed by measurement noise)
255   yMeas = zeros(size(C, 1), simCycles + 1, simRuns);
256 
257   % --- 1f) memory allocation for evaluation variables
258   %
259   % state trajectories estimated by delayed Kalman filter
260   traceXdelayedKF = zeros(size(A, 1), simCycles + 1, simRuns);
261   % state trajectories estimated by normal Kalman filter
262   traceXnormalKF = zeros(size(A, 1), simCycles + 1, simRuns);
263   % state trajectories estimated by proposed IMM algorithm
264   traceXdelayedImm = zeros(size(A, 1), simCycles + 1, simRuns);
265   % inverted pendulum setpoint trajectory
266   traceSetpoint = zeros(size(C, 1), simCycles + 1, simRuns);
267   % debug variables
268   traceDebug1 = zeros(size(A, 1), simCycles + 1, simRuns);
269   traceDebug2 = zeros(size(A, 1), simCycles + 1, simRuns);
270   traceDebug3 = zeros(size(A, 1), simCycles + 1, simRuns);
271   
272 
273   %% ----------------------------------------------------------------------
274   % ---------- 2) Monte Carlo Simulation: Outer Loop
275   % ----------    Initializing a New Run
276   % -----------------------------------------------------------------------
277   for run = 1 : simRuns
278     % display run number
279     run %#ok<NOPRT>
280 
281     % 2a) Init data NETWORKS and network related parameters
282     %
283     % randomly choose one delay distribution of the given distributions
284     uDelProbsNw = uDelProbsNwMc{randi(numel(uDelProbsNwMc), 1)};
285     yDelProbsNw = yDelProbsNwMc{randi(numel(uDelProbsNwMc), 1)};
286     % set network paramters for maximal time delay;
287     % it is chosen so that the last entry of the time delay probability 
288     % distribution vector uDelProbsNw corresponds to the loss rate of the
289     % network. The max(..) expression prevents a negative maximal delay 
290     % in case uDelProbsNW = 1 (perfect connection)
291     uMaxDelayNw = max(0, numel(uDelProbsNw) - 2);
292     yMaxDelayNw = max(0, numel(yDelProbsNw) - 2);
293     % init data network objects
294     inNW = ClassCommNetwork(simCycles, uMaxDelayNw, uDelProbsNw);
295     outNW = ClassCommNetwork(simCycles, yMaxDelayNw, yDelProbsNw);
296     % calculating weighting factors for online calculation of expected
297     % control ipnut in two steps:
298     % - first: calculate transition matrix of time delay distribution;
299     trans = calcDelayTrans(uDelProbsFilter);
300     % - second: deriving weightings as steady state solution of the
301     %           transistion matrix (eigenvector of eigenvalue 1)
302     [eigenVec, eigenVal] = eig(trans');
303     % bounds have to be inserted because of rounding errors
304     [row, ~] = find((eigenVal > (1 - 0.00000001)) & ...
305       (eigenVal < (1 + 0.00000001)));
306     if isempty(row)
307       error(['** Transition matrix for time delays has no steady' ...
308         'state solution. Please check delay probabilities! **']);
309     else
310       % weightings are the normalized eigenvector
311       weight = eigenVec(:, row) ./ sum(eigenVec(:, row));
312     end
313     % init netowrk data packet objects
314     uPacket = ClassDataPacket();
315     yPacket = ClassDataPacket();
316 
317     % 2b) Init Pendulum Parameter
318     %
319     % initial state
320     xRealMean = xRealMeanMc;
321     xRealCov = diag([1 1 1 1]) .* xRealCovMc(randi(numel(xRealCovMc), 1));
322     % noise variance parameter initialization
323     % process noise
324     wCov = [wVarXMc(randi(numel(wVarXMc), 1)) 0 0 0; ...
325       0 0 0 0; 0 0 wVarPhiMc(randi(numel(wVarPhiMc), 1)) 0; 0 0 0 0];
326     % measurement noise
327     vCov = [vVarXMc(randi(numel(vVarXMc), 1)) 0; ...
328       0 vVarPhiMc(randi(numel(vVarPhiMc), 1))];
329     
330     % 2c) Init State Estimator Parameter
331     %
332     % length of control input sequence considered by estimator
333     uSeqLengthFilter = numel(uDelProbsFilter);
334     % maximal network time delays considered by estimator
335     uMaxDelayFilter = max(0, numel(uDelProbsFilter) - 2);
336     yMaxDelayFilter = max(0, numel(yDelProbsFilter) - 2);
337     % init objects:
338     % - normal Kalman Filter as general reference
339     kf = ClassKF(A, B, C, wCov, vCov, xRealMean, xRealCov);
340     uhis = zeros(size(B, 2), uSeqLengthFilter, uSeqLengthFilter);
341     % - Kalman Filter with delayed measurments
342     dKF = ClassDelayedKF(A, B, C, wCov, vCov, xRealMean, xRealCov, ...
343       yMaxDelayFilter, uDelProbsFilter);
344     % - IMM estimator
345     dIMM = ClassDelayedIMM(A, B, C, wCov, vCov, xRealMean, xRealCov, ...
346       yMaxDelayFilter, uDelProbsFilter);
347 
348     % 2d) Init Actuator
349     %
350     % buffer length of actuator depends on controller output and not on the
351     % real network distribution ->
352     bufLength = numel(uDelProbsFilter);
353     % value applied if buffer runs empty
354     defaultvalue = 0;
355     % dimension of one control input
356     uDim = size(B, 2);
357     % init actuator object
358     actuator = ClassActuator(bufLength, uDim, defaultvalue);
359 
360     % 2e) Init Simulation Variables
361     %
362     % Init monitoring and system variables
363     traceXnormalKF(:, 1, run) = xRealMean;
364     traceXdelayedKF(:, 1, run) = xRealMean;
365     traceXdelayedImm(:, 1, run) = xRealMean;
366     traceDebug1(:, 1, run) = xRealMean;
367     traceDebug2(:, 1, run) = xRealMean;
368     traceDebug3(:, 1, run) = xRealMean;
369     xReal(:, 1, run) = xRealMean;
370     wReal = randn(size(A, 1), simCycles + 1);
371     vReal = randn(size(C, 1), simCycles + 1);
372 
373     %% --------------------------------------------------------------------
374     % ---------- 3) Monte Carlo Simulation: Inner Loop
375     % ----------    Execute One Simulation Run
376     % ---------------------------------------------------------------------
377     for k = 1 : simCycles
378       % 3a) Sequence-Based LQR-Controller:
379       %     The basic controller is a LQR with adjustable setpoint. A
380       %     sequence is generated by predicting the future control inputs
381       %     and lumping all predicted inputs into one data packet
382       %
383       % Setpoint handling: Reverse setpoint after <setPointReverseSteps>
384       if mod(k, setPointReverseSteps) == 0; setpoint = -setpoint; end
385       % tracing setpoint trajectory for debug
386       traceSetpoint(:, k, run) = setpoint;
387       % Solving setpoint equation for adjustable setpoint
388       helpMe = [(A - eye(size(A,1))), B; C, D] \ ...
389         [zeros(size(A, 1), 1); setpoint];
390       % init helping variable that holds the next predicted state
391       xPred = xReal(:, k, run);
392       % generating control input sequence:
393       % - first entry
394       ctrlSeq = -L * (xPred - helpMe(1:size(A, 1), 1)) + ...
395         helpMe(size(A,1) + 1:end, 1);
396       % - all other entries
397       if uSeqLengthFilter >= 2 
398         for i = 2 : uSeqLengthFilter - 1
399           xPred = A * xPred + B * ctrlSeq(i - 1) ;
400           ctrlSeq = [ ctrlSeq, -L * (xPred - helpMe(1:size(A, 1), 1)) + ...
401             helpMe(size(A,1) + 1 : end, 1)];
402         end
403       end
404       % - writing into network data object
405       uPacket.value = [ctrlSeq, 0];
406 
407       
408       % 3b) Controller-Actuator-Network and Actuator
409       %
410       [numPacketsU, uPacketReceived] = inNW.DataInDataOut(uPacket);
411       uReal(:, k, run) = ...
412         actuator.DataInDataOut(numPacketsU, uPacketReceived);
413 
414       
415       % 3c) Plant
416       %
417       % state equation
418       xReal(:, k + 1, run) = ...
419         A * xReal(:, k, run) + B * uReal(:, k, run) + wCov * wReal(:, k);
420       % plant ouput
421       yReal(:, k + 1, run) = C * xReal(:, k + 1, run);
422       % noisy measurement
423       yMeas(:, k + 1, run) = yReal(:, k + 1, run) + vCov * vReal(:, k + 1);
424 
425       
426       % 3d) Sensor-Estimator-Network
427       %
428       % Writing measurement into data packet
429       yPacket.value =  yMeas(:, k + 1, run);
430       % Giving data into network
431       [numPacketsY, yPacketReceived] = outNW.DataInDataOut(yPacket);
432 
433       
434       % 3e) Estimators
435       %
436       % - Normal Kalman filter
437       % calculate expected value of applied control input to have an 
438       % easy estimation of the control input
439       uhis(:, :, 2:end) = uhis(:, :, 1 : (end - 1));
440       uhis(:, :, 1) = uPacket.value;
441       uExp = zeros(size(B, 2), 1);
442       for counter = 1 : 1 : uSeqLengthFilter
443         uExp = uExp + uhis(:, counter, counter) .* weight(counter);
444       end
445       uExp = uExp';
446       % Execute normal Kalman filter
447       traceXnormalKF(:, k + 1, run) = ...
448         kf.Execute(uExp, yPacketReceived(1).value);
449       %
450       % - Kalman filter considering networks
451       traceXdelayedKF(:, k + 1, run) = ...
452         dKF.Execute(uPacket, numPacketsY, yPacketReceived);
453       %
454       % - proposed Interacting Multiple Model Estimator
455       traceXdelayedImm(:, k + 1, run) = ...
456         dIMM.Execute(uPacket, numPacketsY, yPacketReceived);
457     end
458   end
459   
460   %% ----------------------------------------------------------------------
461   %  -------------- 4) Evaluation and Output
462   %  ----------------------------------------------------------------------
463   % 
464   % 4a) Transfrom trajectories of angles from rad to degree
465   xReal(3, :, :) = xReal(3, :, :) * 360 / 2 / pi;
466   xReal(4, :, :) = xReal(4, :, :) * 360 / 2 / pi;
467   yReal(2, :, :) = yReal(2, :, :) * 360 / 2 / pi;
468   yMeas(2, :, :) = yMeas(2, :, :) * 360 / 2 / pi;
469   traceXnormalKF(3, :, :) = traceXnormalKF(3, :, :) * 360 / 2 / pi;
470   traceXnormalKF(4, :, :) = traceXnormalKF(4, :, :) * 360 / 2 / pi;
471   traceXdelayedKF(3, :, :) = traceXdelayedKF(3, :, :) * 360 / 2 / pi;
472   traceXdelayedKF(4, :, :) = traceXdelayedKF(4, :, :) * 360 / 2 / pi;
473   traceXdelayedImm(3, :, :) = traceXdelayedImm(3, :, :) * 360 / 2 / pi;
474   traceXdelayedImm(4, :, :) = traceXdelayedImm(4, :, :) * 360 / 2 / pi;
475 
476   % 4b) Show sample state trajectory and the estimates of the this 
477   %     trajectory
478   clf(figure(1))
479   metric = {'$x$ in m', '$\Delta x/ \Delta t$ in m/s', ...
480       '$\phi$ in $^{\circ}$', '$\omega$ in $^{\circ}/s$'};
481   for pNr = 1 : size(A, 1)
482     figure(1)
483     subplot(size(A, 1), 1, pNr);
484     ylabel(metric(pNr));
485     hold;
486     plot((1:simCycles + 1) * Ts, xReal(pNr, :, 1), 'b')
487     %  plot((1:simCycles + 1) * Ts, traceXnormalKF(pNr, :, 1), 'c')
488     plot((1:simCycles + 1) * Ts, traceXdelayedKF(pNr, :, 1), 'k')
489     plot((1:simCycles + 1) * Ts, traceXdelayedImm(pNr, :, 1), 'r')
490     legend('real state', 'sKF', 'IMM');
491   end
492   xlabel('$t$ in s');
493   drawnow
494 
495   % 4c) Show sample plant output and measurement trajectory for that
496   clf(figure(2))
497   metric = {'$x$ in m ', '$\phi$ in $^{\circ}$'};
498   for pNr = 1 : size(C, 1)
499     figure(2)
500     subplot(size(C, 1), 1, pNr);
501     ylabel(metric(pNr));
502     hold;
503     plot((1:simCycles + 1) * Ts, yReal(pNr, :, 1), 'k')
504     plot((1:simCycles + 1) * Ts, yMeas(pNr, :, 1), 'g')
505     legend('yReal', 'yMeas');
506   end
507   xlabel('$t$ in s');
508   drawnow
509   
510   % 4d) Show sample trajectroy of the control inputs
511   clf(figure(3))
512   figure(3)
513   plot((1:simCycles +1 ) * Ts, uReal(1, :, 1), 'b');
514   drawnow
515   
516   % 4e) Compare the averaged RMSE trajectories
517   %
518   % memory allocation for RMSE variables
519   rmseKF   = zeros(size(A, 1), simCycles);
520   rmseDKF  = zeros(size(A, 1), simCycles);
521   rmseDIMM = zeros(size(A, 1), simCycles);
522   % calculating RMSE (root mean squared error) over time
523   for xDim = 1 : size(A, 1)
524     for k = 1 : simCycles
525     % rmseKF(xDim, k) = sqrt(mean((traceXnormalKF(xDim, k, 1:run) - ...
526     %   xReal(xDim, k, 1:run)).^2));
527       rmseDKF(xDim, k) = sqrt(mean((traceXdelayedKF(xDim, k, 1:run) - ...
528         xReal(xDim, k, 1:run)).^2));
529       rmseDIMM(xDim, k) = sqrt(mean((traceXdelayedImm(xDim, k, 1:run) - ...
530         xReal(xDim, k, 1:run)).^2));
531     end
532   end
533   % Show RMSE results
534   clf(figure(4))
535   metric = {'RMSE$(x)$ in m', 'RMSE$(\Delta x/ \Delta t)$ in m/s', ...
536       'RMSE$(\phi)$ in $^{\circ}$', 'RMSE$(\omega)$ in $^{\circ}$/s'};
537   for pNr = 1 : size(A, 1)
538       figure(4)
539       subplot(size(A, 1), 1, pNr);
540       ylabel(metric(pNr));
541       hold;
542       % plot((1:simCycles) * Ts, rmseKF(pNr, :), 'c','LineWidth', 1);
543       plot((1:simCycles) * Ts, rmseDKF(pNr, :), 'k','LineWidth', 1);
544       plot((1:simCycles) * Ts, rmseDIMM(pNr, :),'r','LineWidth', 1);
545       legend('sKF', 'IMM');
546   end
547   xlabel('$t$ in s');
548   drawnow
549 end
Download algorithm (11 files) as ZIP

Comments

Please login to post a comment.