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