CloudRunner

... share your algorithms as a web service

Algorithm: Hypothesizing Distributed Kalman Filter

Description:
Reference implementation of the hypothesizing distributed Kalman filter with a simple example.
Tags: Distributed Estimation Kalman Filter Estimator
Usage: Algorithm is public.
Viewed 2882 times, called 90 times
Upload:
Empty star Empty star Empty star Empty star Empty star
0 votes
Marc Reinhardt
04/11/2013 1:55 p.m. (version #3)

Select Version

Version #3: 04/11/2013 1:55 p.m.
Changelog:
Fixed +/- in geometric series in MSE error approximation

Version #1: 02/27/2013 7:51 p.m.
No changelog available

Run Algorithm

:
number of sensors in the network
:
number of time steps the model is evaluated
:
probability that transmission from sensor to fusion node fails
:
probability that sensor has malfunction and is restarted
:
number of time steps after the HGMM is adaptively updated (set to -1 to disable)
:
number of time steps until a new calculated HGMM is employed
: 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/109/hypothesizing-distributed-kalman-filter/version/3/')

    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 classdef SimpleNetwork < HKF_Network 
 2 	properties (Access = protected)
 3 		fusion_node;	% fusion node
 4 		sensors;		% cell array with sensors
 5 	end
 6 	
 7 	methods
 8 		function init(obj, sensors, fusion_node)
 9 			obj.sensors = sensors;
10 			obj.fusion_node = fusion_node;
11 		end
12 	
13 		function sendToFusionNode(obj, sensor_idx, k_cur, x, Delta, varargin)
14 			obj.fusion_node.addEstimate(sensor_idx, k_cur, x, Delta, varargin{:});
15 		end
16 		
17 		function restartSensor(obj, sensor_idx, k_cur, CxInit, Cz)
18 			obj.sensors{sensor_idx}.init(obj, sensor_idx, k_cur, CxInit, Cz);
19 		end
20 		
21 		function broadcastNewHGMM(obj, k_update, CzNew)
22 			for i=1:length(obj.sensors)
23 				obj.sensors{i}.updateHGMM(k_update, CzNew);
24 			end
25 		end
26 	end
27 end
  1 classdef HKF_Sensor < handle
  2 	properties (Access = protected)
  3         % network
  4 		g_network_object;	% an implementation of HKF_Network
  5         
  6 		% common
  7 		sensor_idx;		% index of the sensor
  8 		k_cur;			% current time step
  9 		Cx;				% auxiliary variable ("global covariance")
 10 		Cz;				% HGMM
 11 		x;				% sensor estimate
 12 		Delta;			% delta matrix
 13 		initalized;		% is set to one when at least one measurement or estimate has been added
 14 		% % %
 15 		% OPTIONAL: bound calculations
 16 		Bx;				% Bx matrix
 17 		Bw;				% Bw matrix
 18 		
 19 		% % %
 20 		% OPTIONAL: adaptive improvement of HGMM
 21 		k_next_update;	% time step when HGMM is replaced. -1 for never
 22 		Cz_New;			% new HGMM
 23 	end
 24 	
 25     methods	
 26 		%
 27 		% Initialize the sensor node
 28 		% 
 29 		% @param network_object 	an implementation of HKF_Network
 30         % @param sensor_idx unique index of the sensor node
 31 		% @param k_cur		current time step
 32 		% @param CxInit		an arbitrary covariance matrix that must initially be the same at all nodes
 33 		%					and should reflect the steady state covariance of the KF with global
 34 		%					measurement model CzInit best possible.
 35 		% @param CzInit		initial value for the HGMM
 36 		function init(o, network_object, sensor_idx, k_cur, CxInit, CzInit)
 37 			dimX = length(CxInit);
 38 			
 39             o.g_network_object = network_object;
 40             
 41 			o.initalized = 0;
 42 			o.sensor_idx = sensor_idx;
 43 			o.k_cur = k_cur;
 44 			o.Cx = CxInit;
 45 			o.Cz = CzInit;
 46 			
 47 			o.x = zeros(dimX,1);
 48 			o.Delta = zeros(dimX,dimX);
 49 			o.Bx = zeros(dimX,dimX);
 50 			o.Bw = zeros(dimX,dimX);
 51 			
 52 			o.k_next_update = -1;
 53 		end
 54 	
 55 		%
 56 		% Predict the available estimate to the next time step
 57 		%
 58 		% @param A			state transition matrix
 59 		% @param Cw			state transition uncertainty
 60 		function process(o, A, Cw)
 61             dimX = length(Cw);
 62 			o.k_cur = o.k_cur + 1;
 63 			
 64 			% update cz
 65 			if(o.k_next_update~=-1 && o.k_cur == o.k_next_update)
 66 				o.Cz = o.Cz_New;
 67 			end
 68 			
 69 			CxPredInv = eye(dimX)/(A * o.Cx * A' + Cw);
 70 			o.Cx = eye(dimX)/(CxPredInv + o.Cz);
 71 			M = o.Cx * CxPredInv * A;
 72 			A_Inv = eye(dimX)/A;
 73 			
 74 			% update variables
 75 			o.x = M * o.x ; 
 76 			o.Delta = M * o.Delta * A_Inv;
 77             o.Bx = M * o.Bx * M';
 78 			o.Bw = M * o.Bw * M' + o.Delta * Cw * o.Delta';
 79 		end
 80 
 81 	
 82 		%
 83 		% Add estimate information to the local estimate
 84 		% 
 85 		% @param xInit 		initial estimate
 86 		% @param CxEstInit 	initial covariance matrix
 87 		function addEstimate(o, xInit, CxEstInit)   
 88 			dimX = length(CxEstInit);
 89 			o.addMeasurement(xInit, eye(dimX), CxEstInit);
 90 		end
 91 		
 92 		%
 93 		% Add the information of a new measurement to the local estimate
 94 		%
 95 		% @param z 			measurement
 96 		% @param H 			measurement matrix
 97 		% @param Cv			measurement noise covariance matrix
 98 		function addMeasurement(o, z, H, Cv)
 99 			L = o.Cx * H' / Cv;
100 			
101 			o.initalized = 1;
102 			o.x = o.x + L * z;
103 			o.Delta = o.Delta + L * H;
104 			o.Bx = o.Bx  + L * Cv * L';
105 		end
106 		
107 		%
108 		% Update the HGMM
109 		%
110 		% @param k_to_update 	time step when new HGMM should be used the first time
111 		% @param CzNew 			new HGMM
112 		function updateHGMM(o, k_to_update, CzNew)
113 			o.k_next_update = k_to_update;
114 			o.Cz_New		= CzNew;
115 		end
116 		
117 		%
118 		% Send data to the fusion node
119 		%
120 		function sendNetworkPacket(o)
121 			if o.initalized == 0
122 				warning('HKF:NOT_INITIALIZED','Could not send sensor estimate. No information contained.');
123 				return;
124 			end
125             
126             o.g_network_object.sendToFusionNode(o.sensor_idx, o.k_cur, o.x, ...
127                 o.Delta, HKF_Network.getchecksum(o.Cx), o.Bx, o.Bw);
128 			end
129 	end
130 end
 1 classdef HKF_Network < handle
 2 	methods(Abstract)
 3 		% cx_checksum, Bx, Bw
 4 		sendToFusionNode(obj, sensor_idx, k_cur, x, Delta, varargin)
 5 		restartSensor(obj, sensor_idx, k_cur, CxInit, Cz)
 6 		broadcastNewHGMM(obj, k_update, CzNew)
 7     end
 8     
 9     methods (Static, Access = public)
10         function checksum = getchecksum(M)
11 			checksum = size(M,1) * size(M,2) * sum(sum(M)) + min(max(M)) + max(min(M));
12 	    end 
13     end
14 end
  1 classdef HKF_FusionNode < handle
  2 	properties (Access = protected)
  3 		NUM_TS_TO_STORE    = 10;  % max time steps old models are stored for out-of-sequence estimates
  4 		K_UPDATE_FREQUENCY = 10;  % set to -1 to prevent updating
  5 		K_UPDATE_DELAY	   = 3;   % time steps between calculation and application of new HGMM
  6 
  7 		% network
  8 		g_network_object;	% an implementation of HKF_Network
  9 
 10 		% common
 11 		k_cur;			% current time step
 12 		Cx;				% auxiliary variable ("global covariance")
 13 		Cz;				% HGMM
 14 		x_vec;			% sensor estimates
 15 		Delta_vec;		% delta matrices of sensors
 16 		k_vec;			% time step when estimate has been sent by sensor
 17 		init_vec;		% helper variable to store which estimates are initialized
 18 		
 19 		% % %
 20 		% OPTIONAL: bound calculations
 21 		Bx_vec;		% Bx matrices of sensors
 22 		Bw_vec;		% Bw matrices of sensors
 23 
 24 		% % %
 25 		% OPTIONAL: out-of-sequence estimates
 26 		% matrices of time steps k_hist_begin to k_cur-1 are stored in matrices 
 27 		k_hist_begin;	 % models are available for k_hist_begin+1
 28 		cx_checksum_hist;% Cx checksum history 
 29 		M_hist;			 % M = K_{t+1} * A_t 
 30 		Cw_hist			 % Cw_t 
 31 		A_Inv_hist;		 % inv{A_t} 
 32 		
 33 		% % %
 34 		% OPTIONAL: adaptive improvement of HGMM
 35 		Cz_New;
 36 	end
 37     
 38     methods
 39 		%
 40 		% Initialize the fusion node
 41 		% 
 42 		% @param network_object 	an implementation of HKF_Network
 43 		% @param k_cur          current time step
 44 		% @param nNodes         number of sensors in the network
 45 		% @param CxInit         an arbitrary covariance matrix that must initially be the same at all nodes
 46 		%                       and should reflect the steady state covariance of the KF with global
 47 		%                       measurement model CzInit best possible.
 48 		% @param CzInit         initial value for the HGMM
 49         % @param varargin{1}    number of time steps old models are stored
 50         % @param varargin{2}    number of time steps after the HGMM is
 51         %                       adaptively updated (set to -1 to disable)
 52         % @param varargin{3}    number of time steps until a new calculated
 53         %                       HGMM is employed.
 54         function init(o, network_object, k_cur, nNodes, CxInit, CzInit, varargin)   
 55 			dimX = length(CxInit);
 56 			
 57 			o.g_network_object = network_object;
 58 			o.k_cur = k_cur;
 59 			o.Cx = CxInit;
 60 			o.Cz = CzInit;
 61 			o.x_vec = zeros(dimX*nNodes,1);
 62 			o.Delta_vec = zeros(dimX*nNodes,dimX);
 63 			o.k_vec = -ones(nNodes,1);
 64 			o.init_vec = zeros(nNodes, 1);
 65 			
 66 			o.Bx_vec = zeros(dimX*nNodes,dimX);
 67 			o.Bw_vec = zeros(dimX*nNodes,dimX);
 68 			
 69 			o.k_hist_begin = k_cur;
 70 			o.cx_checksum_hist = {}; 
 71 			o.M_hist = {}; 
 72 			o.Cw_hist = {}; 
 73 			o.A_Inv_hist = {}; 
 74             
 75 			o.Cz_New = CzInit;
 76             
 77             % set optional parameters
 78             if(~isempty(varargin)) 
 79                 o.NUM_TS_TO_STORE = varargin{1}; end
 80             if(length(varargin)>1) 
 81                 o.K_UPDATE_FREQUENCY = varargin{2}; end
 82             if(length(varargin)>2) 
 83                 o.K_UPDATE_DELAY = varargin{3}; end
 84         end
 85         
 86 		%
 87 		% Add an estimate of a sensor to the fusion node
 88 		%
 89 		% @param sensor_idx		sensor id
 90 		% @param k 				time stamp of estimate
 91 		% @param x 				estimate
 92 		% @param Delta 			correction matrix
 93 		% @param varargin{1} 	cx checksum
 94 		% @param varargin{2} 	Bx
 95 		% @param varargin{3} 	Bw
 96 		function addEstimate(o, sensor_idx, k, x, Delta, varargin)
 97 			% check if estimate is older than an earlier received one
 98 			if k <= o.k_vec(sensor_idx)
 99 				warning('HKF:OLD_MSG','Received old measurement. Drop...');
100 				return;
101 			end
102 			
103 			% check if our out of sequence processing has enough data
104             if k ~= o.k_cur && k <= o.k_hist_begin
105 				warning('HKF:OOS_LIMITED','Cannot include estimate. Old models have been dropped');
106 				return;
107             elseif k > o.k_cur
108                 warning('HKF:OOS_LIMITED','Cannot include estimate. Fusion node time smaller than estimate time');
109                 return;
110             end
111 			
112             
113 			% check if checksum code of estimate matches the one of the fusion node
114 			cx_checksum = HKF_Network.getchecksum(o.Cx);
115             if(k < o.k_cur)
116                 cx_checksum = o.cx_checksum_hist{k - o.k_hist_begin + 1};
117             end
118 			if ~isempty(varargin) && abs(varargin{1}-cx_checksum) > 1e-6
119 				warning('HKF:CX_checksum','Cx checksum does not match with fusion center. k = %d',k);
120 				o.g_network_object.restartSensor(sensor_idx, o.k_cur, o.Cx, o.Cz);
121 				return;
122 			end				
123 			
124 			% set estimate state to initialized
125 			o.init_vec(sensor_idx) = 1;
126 			
127 			% handle optional parameters
128 			dimX = length(Delta);
129 			Bx = zeros(dimX);
130 			Bw = zeros(dimX);
131 			if(length(varargin)>1)
132 				Bx = varargin{2};
133 			end 
134 			if(length(varargin)>2)
135 				Bw = varargin{3};
136 			end
137 
138 			% predict to current time step
139 			[x, Delta, Bx, Bw] = o.predict(k, o.k_cur, x, Delta, Bx, Bw);
140 			 
141 			% update parameters			
142 			xIdx = dimX * (sensor_idx-1) + 1 : dimX * sensor_idx;
143 			o.x_vec(xIdx) = x; 
144 			o.Delta_vec(xIdx,:) = Delta;
145 			o.k_vec(sensor_idx) = k;
146 			o.Bx_vec(xIdx,:) = Bx;
147 			o.Bw_vec(xIdx,:) = Bw;
148 		end
149 		
150 		%
151 		% Predict the available estimates to the next time step and
152 		% calculate a new hypothesis when necessary
153 		%
154 		% @param A				state transition matrix
155 		% @param Cw				state transition uncertainty
156         function process(o, A, Cw)
157 			% predict + filter
158 			dimX = length(Cw);
159 			nEstimates = length(o.x_vec)/dimX;
160                        
161             % set new hypothesis (based on filtered (not predicted) data)
162 			if(o.K_UPDATE_FREQUENCY~=-1 && mod(o.k_cur, o.K_UPDATE_FREQUENCY) == 0 && sum(o.init_vec) > 0)
163                 DeltaFus = repmat(eye(dimX),1,nEstimates) * o.Delta_vec;
164                 deltaFusVector = reshape(DeltaFus, dimX*dimX, 1);
165                 M = (eye(dimX)-o.Cx*o.Cz)*A;
166                 Gain = eye(dimX*dimX) - kron(eye(dimX)/A', M);
167                 o.Cz_New = eye(dimX)/o.Cx * reshape(Gain * deltaFusVector, dimX, dimX);
168 				o.g_network_object.broadcastNewHGMM(o.k_cur + o.K_UPDATE_DELAY, o.Cz_New);
169 			end
170             
171             % update hypothesis when it is time
172 			o.k_cur = o.k_cur + 1;
173             if(o.K_UPDATE_FREQUENCY ~= -1 && ...
174                     mod(o.k_cur,o.K_UPDATE_FREQUENCY) == o.K_UPDATE_DELAY)
175 				o.Cz = o.Cz_New;
176             end
177 			
178             CxOld = o.Cx;
179             CxPred = A * o.Cx * A' + Cw;
180 			o.Cx = eye(dimX)/(eye(dimX)/CxPred + o.Cz);
181 			M = o.Cx / CxPred * A; % K * A;
182 			A_Inv = eye(dimX)/A;
183             
184 			% set history variables
185 			if(length(o.cx_checksum_hist) >= o.NUM_TS_TO_STORE)
186 				o.k_hist_begin = o.k_hist_begin + 1;
187 				o.cx_checksum_hist(1) = [];
188 				o.M_hist(1) = [];
189 				o.Cw_hist(1) = [];
190 				o.A_Inv_hist(1) = [];
191 			end
192 			newIdx = length(o.cx_checksum_hist) + 1;
193 			o.cx_checksum_hist{newIdx} = HKF_Network.getchecksum(CxOld);
194 			o.M_hist{newIdx} = M;
195 			o.Cw_hist{newIdx} = Cw;
196 			o.A_Inv_hist{newIdx} = A_Inv;
197 	
198 			% predict estimates to current time step
199             for i = 1 : nEstimates
200                 if o.init_vec(i) == 0 % not yet initialized
201 					continue;
202                 end
203 				xIdx = dimX * (i-1) + 1 : dimX * i;
204 				[o.x_vec(xIdx), o.Delta_vec(xIdx,:), o.Bx_vec(xIdx,:), o.Bw_vec(xIdx,:)] = o.predict(o.k_cur-1, ...
205 					o.k_cur, o.x_vec(xIdx), o.Delta_vec(xIdx,:), o.Bx_vec(xIdx,:), o.Bw_vec(xIdx,:));
206             end
207 		end   
208 	   
209 	    %
210 		% Return the fused estimate
211 	    function [x, CxApprox, CxBound] = getEstimate(o)
212 			dimX = length(o.Cx);
213 			nEstimates = length(o.x_vec)/dimX;
214 			
215 			if sum(o.init_vec) == 0
216 				error('HKF:NOT_INITIALIZED','No estimate has been initialized. Cannot obtain fused estimate');
217 			end
218 			
219 			x_fus = zeros(dimX,1);
220 			Delta_fus = zeros(dimX,dimX);
221 			Bx_fus = zeros(dimX,dimX);
222 			Bw_fus = zeros(dimX,dimX);
223 			for i=1:nEstimates
224 				if(o.init_vec(i) == 0) % not yet initialized
225 					continue;
226 				end
227 				xIdx = dimX * (i-1)+1:dimX * i;
228 				x_fus = x_fus + o.x_vec(xIdx);
229 				Delta_fus = Delta_fus + o.Delta_vec(xIdx,:);
230 				Bx_fus = Bx_fus + o.Bx_vec(xIdx,:);
231 				Bw_fus = Bw_fus + o.Bw_vec(xIdx,:);
232 			end
233 			
234 			if(cond(Delta_fus)> 1e+15)
235 				error('HKF:NOT_INITIALIZED','Delta matrix is close to singular. Cannot obtain estimate.');
236 			end
237 			Delta_fus_Inv = eye(dimX) / Delta_fus;
238 			
239 			% Bw approximation
240 			tLastIdx = length(o.cx_checksum_hist);
241 			M = o.M_hist{tLastIdx};
242 			N = M * Delta_fus * o.A_Inv_hist{tLastIdx} * o.Cw_hist{tLastIdx} * ...
243 				o.A_Inv_hist{tLastIdx}' * Delta_fus' * M';
244 			normM = max(max(M));
245 			Bw_approx = N;
246 			for i = 1 : o.k_cur - 1
247 				normRest = max(max(N)) * normM.^2 * (1 - normM.^(2*o.k_cur - 2*i)) / (1 - normM.^2) ;
248 				normBw = max(max(Bw_approx));
249 				if(normRest / normBw   < 0.005)
250 					break;
251 				end
252 				N = M * N * M';				
253 				Bw_approx = Bw_approx + N;
254 			end
255 			
256 			% set results
257 			x = Delta_fus_Inv * x_fus;
258 			CxApprox = Delta_fus_Inv * (Bx_fus + Bw_approx) * Delta_fus_Inv';
259 			CxBound = Delta_fus_Inv * (Bx_fus + nEstimates * Bw_fus) * Delta_fus_Inv';
260 		end
261 	end
262 
263 	methods(Access = protected)
264        function [x, Delta, Bx, Bw] = predict(o, k_from, k_to, x, Delta, Bx, Bw)
265            if(k_from == k_to)
266                return;
267            end
268            
269            if k_from < o.k_hist_begin
270                error('HKF:OOS_LIMITED','Could not predict estimates. OOS data not old enough.');
271             end
272             
273             tIdx = k_from - o.k_hist_begin + 1;
274             M = o.M_hist{tIdx};
275             Cw = o.Cw_hist{tIdx};
276             A_Inv = o.A_Inv_hist{tIdx};
277 			for k_temp = k_from : k_to - 1
278 				x = M*x;
279 				Delta = M * Delta * A_Inv ;
280 				Bx = M* Bx *M';
281                 Bw = M * Bw * M' + Delta * Cw * Delta';
282 			end
283        end
284 	end
285 end
 1 % Run the hypothesizing distributed Kalman filter in a simple scenario
 2 % @param NUM_SENSORS			number of sensors in the network
 3 % @param NUM_TIMESTEPS			number of time steps the model is evaluated
 4 % @param PACKET_DROP_RATE		probability that transmission from sensor to fusion node fails
 5 % @param SENSOR_RESTART_PROB	probability that sensor has malfunction and is restarted
 6 % @param K_UPDATE_FREQUENCY		number of time steps after the HGMM is adaptively updated (set to -1 to disable)
 7 % @param K_UPDATE_DELAY			number of time steps until a new calculated HGMM is employed.
 8 function RunExample(NUM_SENSORS, NUM_TIMESTEPS, PACKET_DROP_RATE, SENSOR_RESTART_PROB, K_UPDATE_FREQUENCY, K_UPDATE_DELAY)
 9 	rng('shuffle');
10 	
11 	SENSOR_RESTART_PROB = SENSOR_RESTART_PROB / 100;
12 	PACKET_DROP_RATE = PACKET_DROP_RATE / 100;
13 		
14 	% suppress spam messages
15 	if(SENSOR_RESTART_PROB > 0)
16 		warning('off','HKF:CX_CHECKSUM')
17 	end
18 
19 	xTrue = [20;20];
20 	A = [1 1 ; 0 1];
21 	Cw = 10 * eye(2);
22 	H = eye(2);
23 	Cv = eye(2);
24 	CxInit = eye(2);
25 	CzInit = eye(2);
26 
27 	%
28 	% Declaration
29 	%
30 	network = SimpleNetwork;
31 	fusion_node = HKF_FusionNode;
32 	sensors = cell(NUM_SENSORS);
33 
34 	%
35 	% Initialization
36 	%
37 	fusion_node.init(network, 0, NUM_SENSORS, CxInit, CzInit, 10, K_UPDATE_FREQUENCY, K_UPDATE_DELAY);
38 	for i=1:NUM_SENSORS
39 		sensors{i} = HKF_Sensor;
40 		sensors{i}.init(network, i, 0, CxInit, CzInit);
41 	end
42 	network.init(sensors, fusion_node);
43 
44 	%
45 	% Simple estimation scenario
46 	%
47 	xErr = zeros(NUM_TIMESTEPS,1);
48 	cxApprox = zeros(NUM_TIMESTEPS,1);
49 	cxBound = zeros(NUM_TIMESTEPS,1);
50 	for k = 1 : NUM_TIMESTEPS
51 		xTrue = A * xTrue + mvnrnd(zeros(size(Cw,1),1),Cw)';
52 		
53 		% process sensors and fusion node
54 		fusion_node.process(A, Cw);
55 		for i=1:NUM_SENSORS
56 			sensors{i}.process(A, Cw);
57 		end    
58 		
59 		% simulate measurements    
60 		for i=1:NUM_SENSORS
61 			z = H * xTrue + mvnrnd(zeros(size(H,1),1),Cv)';
62 			sensors{i}.addMeasurement(z, H, Cv);
63 		end
64 		
65 		% simulate communication
66 		for i=1:NUM_SENSORS
67 			if rand() > 1 - PACKET_DROP_RATE
68 				continue;
69 			end
70 			sensors{i}.sendNetworkPacket();
71 		end
72 
73 		% simulate sensor crashes
74 		for i=1:NUM_SENSORS
75 			if rand() > SENSOR_RESTART_PROB
76 				continue;
77 			end
78 			% technique: init with wrong CxInit -> restart is triggerd by
79 			% fusion node
80 			sensors{i}.init(network, i, k, CxInit, CzInit);
81 		end
82 		
83 		% set errors
84 		[x, CxApprox, CxBound] = fusion_node.getEstimate();
85 		xErr(k) = (xTrue - x)'*(xTrue - x);
86 		cxApprox(k) = trace(CxApprox);
87 		cxBound(k) = trace(CxBound);
88 	end
89 
90 	%
91 	% Evaluation
92 	%
93 	plot(1:NUM_TIMESTEPS, xErr, ...
94 		1:NUM_TIMESTEPS, cxApprox,...
95 		1:NUM_TIMESTEPS, cxBound);
96 end
Download algorithm (5 files) as ZIP

Comments

Please login to post a comment.