diff --git a/commonFunctions/sensors/Sensor1D.m b/commonFunctions/sensors/Sensor1D.m
index 1eff68a7aadfe8364997055d791298b2c2c2eee6..62528747ad6c84bbf800c7d42ce7fdf48d77cac8 100644
--- a/commonFunctions/sensors/Sensor1D.m
+++ b/commonFunctions/sensors/Sensor1D.m
@@ -5,7 +5,8 @@ classdef Sensor1D < handle
% email: stefano.belletti@skywarder.eu
% Release date: 18/11/2024
%
- % Sensor class for 1D sensors
+ % Sensor class for 1D sensors, adding properties and noise as defined
+ % in initSensorsYYYY_Mission.m
%
% Creating a new sensor: [obj] = Sensor1D()
@@ -18,9 +19,9 @@ classdef Sensor1D < handle
% noises
noiseType; % White (default) or Pink
- noiseDataTrack1;
- noiseFactor;
- colored_opts;
+ noiseDataTrack1; % Noise data for track1 (only data for 1D sensors)
+ noiseFactor; % Scale factor
+ colored_opts; % Colored data noise
noiseVariance; % Defining gaussian white noise
@@ -49,9 +50,11 @@ classdef Sensor1D < handle
function [] = update(obj,varargin)
if isa(obj,'SensorGPS')
+ % Noise is still unknown
error("Do not use .update for class SensorGPS")
end
+ % Check for crucial data
if isempty(obj.minMeasurementRange)
error("Empty minMeasurementRange")
end
@@ -67,8 +70,9 @@ classdef Sensor1D < handle
end
end
- % Noise
+ % Noise initialization
if size(varargin,2) == 3
+ % Find the correct element of the vector
vect = varargin{1};
name = varargin{2};
number = varargin{3};
@@ -105,20 +109,22 @@ classdef Sensor1D < handle
obj.colored_opts.white_variance = vect(ii).colored_data.white_variance;
obj.colored_opts.fcut = vect(ii).colored_data.fcut;
obj.colored_opts.butterOrder = vect(ii).colored_data.butterOrder;
+ % "Filter's memory":
obj.colored_opts.filterStatus1 = 0;
obj.colored_opts.filterStatus2 = 0;
obj.colored_opts.filterStatus3 = 0;
end
else
- if strcmp("Sensor1D", class(obj)) || strcmp("SensorFault", class(obj))
- obj.noiseDataTrack1 = [];
- elseif strcmp("Sensor3D", class(obj)) || strcmp("SensorGPS", class(obj))
- obj.noiseDataTrack1 = [];
- obj.noiseDataTrack2 = [];
- obj.noiseDataTrack3 = [];
- else
- error("Sensor not defined")
- end
+ % if strcmp("Sensor1D", class(obj)) || strcmp("SensorFault", class(obj))
+ % obj.noiseDataTrack1 = [];
+ % elseif strcmp("Sensor3D", class(obj)) || strcmp("SensorGPS", class(obj))
+ % obj.noiseDataTrack1 = [];
+ % obj.noiseDataTrack2 = [];
+ % obj.noiseDataTrack3 = [];
+ % else
+ % error("Sensor not defined")
+ % end
+ error("Sensor not found")
end
end
end
@@ -133,7 +139,8 @@ classdef Sensor1D < handle
outputArg = inputArg;
end
- function outputArg = add2DOffset(obj,inputArg,temp)
+ function outputArg = add2DOffset(obj,inputArg,temp)
+ % Adding 2DOffset if present
if (~isempty(obj.error2dOffset))
inputArg=inputArg + griddata(obj.error2dOffset(:,1),obj.error2dOffset(:,2),obj.error2dOffset(:,3),inputArg,temp);
end
@@ -149,10 +156,10 @@ classdef Sensor1D < handle
end
function outputArg = addNoise(obj,inputArg,t)
- % Add noise
- if ~isempty(obj.noiseVariance) % check for old results
+ % Add noise, depending on the type and magnitude
+ if ~isempty(obj.noiseVariance) % white noise override
inputArg = inputArg + sqrt(obj.noiseVariance).*randn(length(inputArg),1);
- elseif ~isempty(obj.noiseDataTrack1)
+ elseif ~isempty(obj.noiseDataTrack1)
if strcmp(obj.noiseType, "white")
inputArg = inputArg + sqrt(obj.noiseDataTrack1*obj.noiseFactor^2).*randn(length(inputArg),1);
elseif strcmp(obj.noiseType, "pink")
diff --git a/commonFunctions/sensors/Sensor3D.m b/commonFunctions/sensors/Sensor3D.m
index d338de91cd48de2fe0794b96b3969166f3253f72..81e46a3efb8a591b5a91a3f94b25ab89a2a63fa2 100644
--- a/commonFunctions/sensors/Sensor3D.m
+++ b/commonFunctions/sensors/Sensor3D.m
@@ -5,36 +5,40 @@ classdef Sensor3D < Sensor1D
% email: stefano.belletti@skywarder.eu
% Release date: 18/11/2024
%
- % Sensor class for 3D sensors
+ % Sensor class for 3D sensors, adding properties and noise as defined
+ % in initSensorsYYYY_Mission.m
%
% Creating a new sensor: [obj] = Sensor3D()
properties
- noiseDataTrack2;
- noiseDataTrack3;
+ noiseDataTrack2; % Noise data for track2
+ noiseDataTrack3; % Noise data for track3
- offsetX;
- offsetY;
- offsetZ;
+ offsetX; % Offset in X direction
+ offsetY; % Offset in Y direction
+ offsetZ; % Offset in Z direction
- walkDiffusionCoef;
- transMatrix;
+ walkDiffusionCoef; % Random walk coefficient
+ transMatrix; % Alignment matrix
end
properties (Access = 'protected')
- stateWalkX;
- stateWalkY;
- stateWalkZ;
+ % state walk = drift
+ stateWalkX; % Walk in X direction
+ stateWalkY; % Walk in Y direction
+ stateWalkZ; % Walk in Z direction
end
methods (Access = 'public')
function obj = Sensor3D()
- obj.stateWalkX = 0;
- obj.stateWalkY = 0;
- obj.stateWalkZ = 0;
+ obj.stateWalkX = 0; % Initial X walk = 0
+ obj.stateWalkY = 0; % Initial Y walk = 0
+ obj.stateWalkZ = 0; % Initial Z walk = 0
end
function [outputArg1,outputArg2,outputArg3] = sens(obj,inputArg1,inputArg2,inputArg3,temp,t)
+ % Sens loop: here the ideal input is transformed into the real
+ % output
[inputArg1,inputArg2,inputArg3] = obj.tranformAxis(inputArg1,inputArg2,inputArg3);
[inputArg1,inputArg2,inputArg3] = obj.addOffset3D(inputArg1,inputArg2,inputArg3);
[inputArg1,inputArg2,inputArg3] = obj.randomWalk(inputArg1,inputArg2,inputArg3);
@@ -68,7 +72,8 @@ classdef Sensor3D < Sensor1D
end
methods (Access = 'protected')
- function [outputArg1,outputArg2,outputArg3] = addOffset3D(obj,inputArg1,inputArg2,inputArg3)
+ function [outputArg1,outputArg2,outputArg3] = addOffset3D(obj,inputArg1,inputArg2,inputArg3)
+ % Adding 3D offset
if (~isempty(obj.offsetX)) && obj.offsetX
inputArg1 = inputArg1 + ones(size(inputArg1)).*obj.offsetX;
end
@@ -84,6 +89,7 @@ classdef Sensor3D < Sensor1D
end
function [outputArgX,outputArgY,outputArgZ] = randomWalk(obj,inputArgX,inputArgY,inputArgZ)
+ % Adding random walk
if (~isempty(obj.walkDiffusionCoef)) && obj.walkDiffusionCoef
% distance
s = sqrt ( 2.0 * 3 * obj.walkDiffusionCoef * obj.dt ) * randn ( 1, 1);
@@ -111,6 +117,7 @@ classdef Sensor3D < Sensor1D
end
function [outputArg1,outputArg2,outputArg3] = tranformAxis(obj,inputArg1,inputArg2,inputArg3)
+ % Adding transformation
if (~isempty(obj.transMatrix))
transOut=obj.transMatrix*[inputArg1,inputArg2,inputArg3]';
inputArg1 = transOut(1);
@@ -124,11 +131,17 @@ classdef Sensor3D < Sensor1D
end
function [outputArg1,outputArg2,outputArg3] = addNoise3D(obj,inputArg1,inputArg2,inputArg3,t)
- if ~isempty(obj.noiseVariance) % check for old results
+ % Check for all three channels
+ if ~isempty(obj.noiseDataTrack1) + ~isempty(obj.noiseDataTrack2) + ~isempty(obj.noiseDataTrack3) ~= 3
+ error("The 3D sensor does not have 3 noise channels")
+ end
+
+ % Adding noise
+ if ~isempty(obj.noiseVariance) % white noise override
inputArg1 = inputArg1 + sqrt(obj.noiseVariance).*randn(length(inputArg1),1);
inputArg2 = inputArg2 + sqrt(obj.noiseVariance).*randn(length(inputArg2),1);
inputArg3 = inputArg3 + sqrt(obj.noiseVariance).*randn(length(inputArg3),1);
- elseif ~isempty(obj.noiseDataTrack1)
+ elseif ~isempty(obj.noiseDataTrack1) && ~isempty(obj.noiseDataTrack2) && ~isempty(obj.noiseDataTrack3)
if strcmp(obj.noiseType, "white")
inputArg1 = inputArg1 + sqrt(obj.noiseDataTrack1*obj.noiseFactor^2).*randn(length(inputArg1),1);
inputArg2 = inputArg2 + sqrt(obj.noiseDataTrack2*obj.noiseFactor^2).*randn(length(inputArg2),1);
@@ -139,21 +152,12 @@ classdef Sensor3D < Sensor1D
inputArg2 = inputArg2 + obj.noiseDataTrack2.peaks_vect_val(ii)*obj.noiseFactor*sin(2*pi*obj.noiseDataTrack2.peaks_vect_f(ii)*t + randn(1));
inputArg3 = inputArg3 + obj.noiseDataTrack3.peaks_vect_val(ii)*obj.noiseFactor*sin(2*pi*obj.noiseDataTrack3.peaks_vect_f(ii)*t + randn(1));
end
- % Colored noise
- white_noise1 = sqrt(obj.noiseDataTrack1.variance*obj.noiseFactor^2).*randn(1,1);
- [b1, a1] = butter(obj.noiseDataTrack1.butterOrder, obj.noiseDataTrack1.fcut, 'low');
- [colored_noise1, obj.colored_opts.filterStatus1] = filter(b1, a1, white_noise1, obj.colored_opts.filterStatus1);
- white_noise2 = sqrt(obj.noiseDataTrack2.variance*obj.noiseFactor^2).*randn(1,1);
- [b2, a2] = butter(obj.noiseDataTrack2.butterOrder, obj.noiseDataTrack2.fcut, 'low');
- [colored_noise2, obj.colored_opts.filterStatus2] = filter(b2, a2, white_noise2, obj.colored_opts.filterStatus2);
- white_noise3 = sqrt(obj.noiseDataTrack3.variance*obj.noiseFactor^2).*randn(1,1);
- [b3, a3] = butter(obj.noiseDataTrack3.butterOrder, obj.noiseDataTrack3.fcut, 'low');
- [colored_noise3, obj.colored_opts.filterStatus3] = filter(b3, a3, white_noise3, obj.colored_opts.filterStatus3);
+ else
+ error("This noise is not defined")
+ end
- inputArg1 = inputArg1 + colored_noise1;
- inputArg2 = inputArg2 + colored_noise2;
- inputArg3 = inputArg3 + colored_noise3;
- elseif strcmp(obj.noiseType, "colored")
+ if strcmp(obj.noiseType, "pink") || strcmp(obj.noiseType, "colored")
+ % Colored noise: added to pink and colored noise types
white_noise1 = sqrt(obj.noiseDataTrack1.variance*obj.noiseFactor^2).*randn(1,1);
[b1, a1] = butter(obj.noiseDataTrack1.butterOrder, obj.noiseDataTrack1.fcut, 'low');
[colored_noise1, obj.colored_opts.filterStatus1] = filter(b1, a1, white_noise1, obj.colored_opts.filterStatus1);
@@ -167,8 +171,6 @@ classdef Sensor3D < Sensor1D
inputArg1 = inputArg1 + colored_noise1;
inputArg2 = inputArg2 + colored_noise2;
inputArg3 = inputArg3 + colored_noise3;
- else
- error("This noise is not defined")
end
end
diff --git a/commonFunctions/sensors/SensorFault.m b/commonFunctions/sensors/SensorFault.m
index 6c4138289febd75bd98b7bf095c0267714926d12..56055ad20cbbe447bdd4737eee2014a1ee8fac1e 100644
--- a/commonFunctions/sensors/SensorFault.m
+++ b/commonFunctions/sensors/SensorFault.m
@@ -5,7 +5,8 @@ classdef SensorFault < Sensor1D
% email: stefano.belletti@skywarder.eu
% Release date: 18/11/2024
%
- % Sensor class for SensorFault
+ % Sensor class for SensorFault, adding properties and noise as defined
+ % in initSensorsYYYY_Mission.m
%
% Creating a new sensor: [obj] = Sensor1D()
@@ -77,7 +78,6 @@ classdef SensorFault < Sensor1D
end
end
-
function [obj, tError] = setErrorTime(obj) % to set when the fault occurs
obj.getFaultTime()
if obj.fault_time >= 0
@@ -207,7 +207,7 @@ classdef SensorFault < Sensor1D
methods (Access='protected')
- function obj = reset(obj) %method
+ function obj = reset(obj) % method
obj.failureType = 'None';
obj.fault_offset = 0;
obj.lambda = 0;