diff --git a/matlab/GPSaidedINS.m b/matlab/GPSaidedINS.m
index d5fb86cd175348f04ea7a18ead9f11f7e9668553..2d843f37ed451660954d8fe94ae7a4ba7cf5e7d8 100644
--- a/matlab/GPSaidedINS.m
+++ b/matlab/GPSaidedINS.m
@@ -58,16 +58,19 @@ GPS_position=true;
%% Initialize GPS sensor
sensor.state='warm_start_available';
-sensor.howLong=0;
+sensor.fetchfp=0.2;
+sensor.geteph=Inf;
sensor.ephExp=1800.0;
sensor.fp=0;
sensor.eph=5;
+sv=5; %initialize the number of visible satellites
turn=true;
power=400;
energy=0;
-pre_state='startup';
-a=0;
+pre_state='startup'; %initialize previous state of the sensor (used for displaying state changes)
+P_store=0; %store summed valued of the trace of P in time
+sv_store=sv; %store number of visible satellites in time
for k=2:N
@@ -77,18 +80,18 @@ for k=2:N
%%%%%%%%%%%%%%%%%
%% GPS sensor %%
%%%%%%%%%%%%%%%%%
- %number of visible satellites
- sv=5;
+ %randomized number of visible satellites
+ sv = random_satellites(sv);
%control action
- a=[a,sum(sqrt(out_data.diag_P(1:3,k-1)))];
+ P_store=[P_store,sum(sqrt(out_data.diag_P(1:3,k-1)))];
if sum(sqrt(out_data.diag_P(1:3,k-1)))<3.0
turn=false;
else
turn=true;
end
%update sensor state
- pre_state=sensor.state;
- sensor=gps(t(k),sensor,turn,sv);
+% pre_state=sensor.state;
+ sensor=gps_randomized(t(k),sensor,turn,sv); %remove _randomized for worst case
%determine if GPS position is available
GPS_position=strcmp(sensor.state,'position_available');
%compute power consumption
@@ -96,11 +99,11 @@ for k=2:N
energy=energy+power*Ts;
end
-% %print new state and current time if it changes
-% if ~strcmp(pre_state, sensor.state)
-% sensor.state
-% t(k)
-% end
+% %print new state and current time if it changes
+% if ~strcmp(pre_state, sensor.state)
+% sensor.state
+% t(k)
+% end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -159,7 +162,7 @@ for k=2:N
end
figure(11)
-plot(a),grid
+plot(P_store),grid
energy
end
@@ -278,9 +281,33 @@ R=(eye(3)-OMEGA)*R;
q=dcm2q(R);
end
-
-
-
-
-
-
+function sv=random_satellites(sv_old)
+coin=random('uniform',0,1);
+sv=sv_old;
+if sv_old==5 %simulate satellite loss of visibility
+ if coin>0.95
+ sv=6;
+ end
+ if coin<0.1
+ sv=4;
+ end
+end
+if sv_old==4 %simulate satellite loss of visibility
+ if coin>0.9
+ sv=5;
+ end
+ if coin<0.0005
+ sv=3;
+ end
+end
+if sv_old==3 %simulate satellite loss of visibility
+ if coin>0.5
+ sv=4;
+ end
+end
+if sv_old==6 %simulate satellite loss of visibility
+ if coin<0.2
+ sv=5;
+ end
+end
+end
diff --git a/matlab/gps_randomized.m b/matlab/gps_randomized.m
new file mode 100644
index 0000000000000000000000000000000000000000..83e4067589f2caf0272fd70182e2dd163ffe8bf3
--- /dev/null
+++ b/matlab/gps_randomized.m
@@ -0,0 +1,166 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% GPS sensor dynamics %%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%STATE is a struct (called sensor) with the following fields
+% state -[string]the current proceural state: no_info, cold_start, read_ephemeris,
+% position_available, warm_start_avaialable, warm_start
+% fetchfp -[double]at what time freq and phase will be fetched (according to the random extraction)
+% geteph -[double]at what time eph data will be acquired (according to the random extraction)
+% ephExp -[double]expiration time of ephemeris data
+% fp -[int]number of tracked satelliets
+% eph -[int]number of satellites for which ephemeris data are available
+
+%INPUTS
+% -time
+% -state
+% -turn on/off (true=turn_ON, false=turn_OFF)
+% -visible satellites
+
+%OUTPUTS
+% -state
+
+%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function sensor=gps_randomized(time,sensor_in,turn,sv)
+
+%% parameters
+%define how many satellites you need to track given the accuracy requirements
+required_sv= 4;
+%define how long ephemeris data last
+ephDuration=1800.0;
+%bounds for time required for fetching freq and phase
+fp_lower=0.02;
+fp_upper=0.1;
+
+%% initialize output
+sensor = sensor_in;
+
+%% equations
+%you cannot track satellites that you dont see
+sensor.fp=min(sensor_in.fp,sv);
+sensor.eph=min(sensor_in.eph,sv);
+
+%% events
+ephemeris_expired=(time>sensor.ephExp)||(sensor.eph<required_sv); %ephemeris data are expired
+lost_visibility=(sensor_in.fp~=sensor.fp)&&(sensor.fp<required_sv);
+fetched_fp=time>=sensor.fetchfp;
+get_ephemeris=time>sensor.geteph;
+
+%% transitions and updates
+%for each of the states handle eventual transitions and updates
+%updates depend on the spefic transition that is fired
+if strcmp(sensor_in.state,'no_info')
+ if(turn)
+ sensor.state='cold_start';
+ sensor.fetchfp=time+random('uniform',fp_lower,fp_upper);
+ end
+end
+if strcmp(sensor_in.state,'position_available')
+ if(get_ephemeris)
+ sensor.state='position_available';
+ sensor.geteph=time+random('uniform',30,60);
+ sensor.eph=sv;
+ sensor.ephExp=time+ephDuration;
+ end
+ if(lost_visibility)
+ sensor.state='warm_start';
+ sensor.fp=0;
+ sensor.fetchfp=time+random('uniform',fp_lower,fp_upper);
+ %might have to go on to cold_start
+ if(ephemeris_expired)
+ sensor.state='cold_start';
+ sensor.fetchfp=time+random('uniform',fp_lower,fp_upper);
+ sensor.eph=0;
+ end
+ end
+ if(ephemeris_expired)
+ sensor.state='read_ephemeris';
+ sensor.eph=0;
+ sensor.geteph=time+random('uniform',30,60);
+ %might have to go on to cold_start
+ if(lost_visibility)
+ sensor.state='cold_start';
+ sensor.fetchfp=time+random('uniform',fp_lower,fp_upper);
+ end
+ end
+ if(~turn)
+ sensor.state='warm_start_available';
+ sensor.fp=0;
+ end
+end
+if strcmp(sensor_in.state,'read_ephemeris')
+ if(lost_visibility)
+ sensor.state='cold_start';
+ sensor.fetchfp=time+random('uniform',fp_lower,fp_upper);
+ end
+ if(get_ephemeris)
+ sensor.state='position_available';
+ sensor.geteph=time+random('uniform',30,60);
+ sensor.eph=sv;
+ sensor.ephExp=time+ephDuration;
+ end
+ if(~turn)
+ sensor.state='no_info';
+ sensor.fp=0;
+ end
+end
+if strcmp(sensor_in.state,'warm_start')
+ if(lost_visibility)
+ sensor.state='warm_start';
+ sensor.fetchfp=time+random('uniform',fp_lower,fp_upper);
+ %might have to go on to cold_start
+ if(ephemeris_expired)
+ sensor.state='cold_start';
+ sensor.fetchfp=time+random('uniform',fp_lower,fp_upper);
+ sensor.eph=0;
+ end
+ end
+ if(fetched_fp)
+ sensor.state='position_available';
+ sensor.geteph=time+random('uniform',30,60);
+ sensor.fp=sv;
+ end
+ if(ephemeris_expired)
+ sensor.state='cold_start';
+ sensor.fetchfp=time+random('uniform',fp_lower,fp_upper);
+ sensor.eph=0;
+ end
+ if(~turn)
+ sensor.state='warm_start_available';
+ sensor.fp=0;
+ end
+end
+if strcmp(sensor_in.state,'cold_start')
+ if(fetched_fp)
+ sensor.state='read_ephemeris';
+ sensor.geteph=time+random('uniform',30,60);
+ sensor.fp=sv;
+ end
+ if(lost_visibility)
+ sensor.state='cold_start';
+ sensor.fetchfp=time+random('uniform',fp_lower,fp_upper);
+ end
+ if(~turn)
+ sensor.state='no_info';
+ sensor.howLong=time;
+ sensor.fp=0;
+ end
+end
+if strcmp(sensor_in.state,'warm_start_available')
+ if(turn)
+ sensor.state='warm_start';
+ sensor.howLong=time;
+ end
+ if(ephemeris_expired)
+ sensor.state='no_info';
+ sensor.eph=0;
+ end
+end
+
+end
+
+
+
+
+