From b6bb27ae453b485d9c60546bd453d06649841575 Mon Sep 17 00:00:00 2001
From: Claudio Mandrioli <claudio.mandrioli@control.lth.se>
Date: Sun, 7 Oct 2018 23:34:30 +0200
Subject: [PATCH] matlab wip

---
 matlab/GPSaidedINS.m    | 26 +++++++++++++++++++++-----
 matlab/gps_randomized.m | 10 +++++-----
 matlab/plot_data.m      |  5 +++++
 3 files changed, 31 insertions(+), 10 deletions(-)

diff --git a/matlab/GPSaidedINS.m b/matlab/GPSaidedINS.m
index 2d843f3..13ed46e 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 83e4067..9db2d12 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 0a37b18..28e0112 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
 
-- 
GitLab