diff --git a/matlab/GPSaidedINS.m b/matlab/GPSaidedINS.m
index 2d843f37ed451660954d8fe94ae7a4ba7cf5e7d8..13ed46e8f10d8cd038ab8e0ba0aa3b29ce716bc6 100644
--- a/matlab/GPSaidedINS.m
+++ b/matlab/GPSaidedINS.m
@@ -20,6 +20,8 @@
 % out_data.delta_u_h        Estimated IMU biases [accelerometers; gyroscopes]
 % out_data.diag_P           Diagonal elements of the Kalman filter state
 %                           covariance matrix.
+% out_data.energy            overall estimated power consumption
+% out_data.error            average RMS
 %
 %
 % Edit: Isaac Skog (skog@kth.se), 2016-09-06
@@ -58,7 +60,7 @@ GPS_position=true;
 
 %% Initialize GPS sensor 
 sensor.state='warm_start_available';
-sensor.fetchfp=0.2;
+sensor.fetchfp=1.0;
 sensor.geteph=Inf;
 sensor.ephExp=1800.0;
 sensor.fp=0;
@@ -67,6 +69,7 @@ sv=5;                 %initialize the number of visible satellites
 turn=true;
 power=400;
 energy=0;
+P_treshold=settings.P_treshold;
 
 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
@@ -82,9 +85,10 @@ for k=2:N
     %%%%%%%%%%%%%%%%%
     %randomized number of visible satellites
     sv = random_satellites(sv);
+    %sv=5;
     %control action
     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
+    if sum(sqrt(out_data.diag_P(1:3,k-1)))<P_treshold
         turn=false;
     else
         turn=true;
@@ -161,9 +165,21 @@ for k=2:N
     out_data.delta_u_h(:,k)=delta_u_h;
     
 end
-figure(11)
-plot(P_store),grid
-energy
+
+%% plot feedback over P matrix
+%figure(11)
+%plot(P_store),grid
+
+%% output for simulation of energy-accuracy tradeoff 
+out_data.energy=energy;
+xest = out_data.x_h(2,:);
+yest = out_data.x_h(1,:);
+xgps = interp1(in_data.GNSS.t,in_data.GNSS.pos_ned(2,:),in_data.IMU.t,'linear','extrap')';
+ygps = interp1(in_data.GNSS.t,in_data.GNSS.pos_ned(1,:),in_data.IMU.t,'linear','extrap')';
+xerr = xest - xgps; 
+yerr = yest - ygps;
+out_data.error = sqrt(mean(xerr.^2+yerr.^2));
+out_data.P_store=P_store;
 end
 
 
diff --git a/matlab/gps_randomized.m b/matlab/gps_randomized.m
index 83e4067589f2caf0272fd70182e2dd163ffe8bf3..9db2d121adc24f677c3dab5f9a665db996d7e15f 100644
--- a/matlab/gps_randomized.m
+++ b/matlab/gps_randomized.m
@@ -30,8 +30,8 @@ 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;
+fp_lower=0.2;
+fp_upper=1.5;
 
 %% initialize output
 sensor = sensor_in;
@@ -44,8 +44,8 @@ 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;
+fetched_fp=(time>sensor.fetchfp);
+get_ephemeris=(time>sensor.geteph);
 
 %% transitions and updates
 %for each of the states handle eventual transitions and updates
@@ -150,7 +150,7 @@ end
 if strcmp(sensor_in.state,'warm_start_available')
     if(turn)
         sensor.state='warm_start';
-        sensor.howLong=time;
+        sensor.fetchfp=time+random('uniform',fp_lower,fp_upper);
     end
     if(ephemeris_expired)
         sensor.state='no_info';
diff --git a/matlab/plot_data.m b/matlab/plot_data.m
index 0a37b18d983ecfb633af1f63640fad07059c7b19..28e01120b517e093336615688038e8eb0bce1117 100644
--- a/matlab/plot_data.m
+++ b/matlab/plot_data.m
@@ -140,5 +140,10 @@ legend('x', 'y')
 
 positionerr_RMS = sqrt(mean(xerr.^2+yerr.^2))
 figure(2)
+
+figure(9)
+plot(out_data.P_store)
+title('P controlled')
+
 end