![]() |
SensorFusionPURPOSE
Fusion of noisy sensor readings.
SYNOPSIS
function G=SensorFusion(range,SR,Sn)
DESCRIPTION
Fusion of noisy sensor readings. Fuses a set of sensor readings taking into account their noise. The fusion is implemented as a product of the Gaussian defined with the sensor reading as a mean and the sensor noise as covariance. Only valid sensor readings (sensor readings that are not NullPoses) are taken into account in the fusion. Parameters: - range: selects the sub-set of sensors to fuse. - SR: cell array with all the sensor readings. - Sn: cell array with the noise for all sensors. See also NullPose. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function G=SensorFusion(range,SR,Sn) 0002 % Fusion of noisy sensor readings. 0003 % 0004 % Fuses a set of sensor readings taking into account their noise. 0005 % The fusion is implemented as a product of the Gaussian defined with the 0006 % sensor reading as a mean and the sensor noise as covariance. 0007 % Only valid sensor readings (sensor readings that are not NullPoses) 0008 % are taken into account in the fusion. 0009 % 0010 % Parameters: 0011 % - range: selects the sub-set of sensors to fuse. 0012 % - SR: cell array with all the sensor readings. 0013 % - Sn: cell array with the noise for all sensors. 0014 % 0015 % See also NullPose. 0016 0017 0018 first=true; 0019 for i=range 0020 d=SR{i}; 0021 % Some sensor readings are erroneous and have not to be fused 0022 if ~NullPose(d) 0023 if first 0024 G=Gaussian(double(SR{i}),Sn{i}); 0025 first=false; 0026 else 0027 G=G*Gaussian(double(SR{i}),Sn{i}); 0028 end 0029 end 0030 end 0031 0032 if first 0033 % if no valid data was provided we return a Gaussian on a NullPose 0034 G=Gaussian(double(SR{range(1)}),Sn{range(1)}); 0035 end 0036 |