From fb78a80e315da025ad71cfa76775db448db8416d Mon Sep 17 00:00:00 2001
From: Claudio <claudio.mandrioli@control.lth.se>
Date: Tue, 26 Feb 2019 16:00:47 +0100
Subject: [PATCH] divided main files and gps model files for car and cycling
data
---
matlab/GPSaidedINS.m | 334 ------------------
matlab/GPSaidedINS_cycling.m | 9 +-
...{gps_randomized.m => gps_randomized_car.m} | 4 +-
matlab/gps_randomized_cycling.m | 152 ++++++++
matlab/main_car.m | 47 +++
matlab/{main.m => main_cycling.m} | 6 +-
6 files changed, 210 insertions(+), 342 deletions(-)
delete mode 100644 matlab/GPSaidedINS.m
rename matlab/{gps_randomized.m => gps_randomized_car.m} (99%)
create mode 100644 matlab/gps_randomized_cycling.m
create mode 100644 matlab/main_car.m
rename matlab/{main.m => main_cycling.m} (85%)
diff --git a/matlab/GPSaidedINS.m b/matlab/GPSaidedINS.m
deleted file mode 100644
index 5bab3b6..0000000
--- a/matlab/GPSaidedINS.m
+++ /dev/null
@@ -1,334 +0,0 @@
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%
-% function call: out_data=GPSaidedINS(in_data,settings)
-%
-% This function integrates GNSS and IMU data. The data fusion
-% is based upon a loosely-coupled feedback GNSS-aided INS approach.
-%
-% Input struct:
-% in_data.GNSS.pos_ned GNSS-receiver position estimates in NED
-% coordinates [m]
-% in_data.GNSS.t Time of GNSS measurements [s]
-% (in_data.GNSS.HDOP GNSS Horizontal Dilution of Precision [-])
-% (in_data.GNSS.VDOP GNSS Vertical Dilution of Precision [-])
-% in_data.IMU.acc Accelerometer measurements [m/s^2]
-% in_data.IMU.gyro Gyroscope measurements [rad/s]
-% in_data.IMU.t Time of IMU measurements [s]
-%
-% Output struct:
-% out_data.x_h Estimated navigation state vector [position; velocity; attitude]
-% 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
-% Revised: Bo Bernhardsson, 2018-01-01
-%
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function out_data=GPSaidedINS(in_data,settings)
-
-
-% Copy data to variables with shorter name
-u=[in_data.IMU.acc;in_data.IMU.gyro];
-t=in_data.IMU.t;
-
-%% Initialization
-% Initialize the navigation state
-x_h=init_navigation_state(u,settings);
-
-% Initialize the sensor bias estimate
-delta_u_h=zeros(6,1);
-
-% Initialize the Kalman filter
-[P,Q1,Q2,~,~]=init_filter(settings);
-
-
-% Allocate memory for the output data
-N=size(u,2);
-out_data.x_h=zeros(10,N);
-out_data.x_h(:,1)=x_h;
-out_data.diag_P=zeros(15,N);
-out_data.diag_P(:,1)=diag(P);
-out_data.delta_u_h=zeros(6,N);
-out_data.turn(1)=true;
-
-%% Information fusion
-ctr_gnss_data=1;
-GPS_position=true;
-
-%% Initialize GPS sensor
-sensor.state='warm_start_available';
-sensor.fetchfp=1.0;
-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;
-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
-sv_store=sv; %store number of visible satellites in time
-
-for k=2:N
-
- % Sampling period
- Ts=t(k)-t(k-1);
-
- %%%%%%%%%%%%%%%%%
- %% GPS sensor %%
- %%%%%%%%%%%%%%%%%
- %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)))<P_treshold
- turn=false;
- else
- turn=true;
- end
- %update sensor state
-% 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
- if ~(strcmp(sensor.state,'no_info')||strcmp(sensor.state,'warm_start_available'))
- 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
-
- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
- % Calibrate the sensor measurements using current sensor bias estimate.
- u_h=u(:,k)+delta_u_h;
-
- % Update the INS navigation state
- x_h=Nav_eq(x_h,u_h,Ts);
-
- % Get state space model matrices
- [F,G]=state_space_model(x_h,u_h,Ts);
-
- % Time update of the Kalman filter state covariance.
- P=F*P*F'+G*blkdiag(Q1,Q2)*G';
-
- % Defualt measurement observation matrix and measurement covariance
- % matrix
-
- y = in_data.GNSS.pos_ned(:,ctr_gnss_data);
- H = [eye(3) zeros(3,12)];
- R = settings.sigma_gps^2*eye(3);
-
- ind = zeros(1,6); % index vector, describing available measurements
- % Check if GNSS measurement is available
- if t(k)==in_data.GNSS.t(ctr_gnss_data)
- if GPS_position
- ind(1:3)=1;
- end
- % Update GNSS data counter
- ctr_gnss_data=min(ctr_gnss_data+1,length(in_data.GNSS.t));
- end
-
- ind=logical(ind);
- H=H(ind,:);
- y=y(ind);
- R=R(ind,ind);
-
- % Calculate the Kalman filter gain.
- K=(P*H')/(H*P*H'+R);
-
- % Update the perturbation state estimate.
- z=[zeros(9,1); delta_u_h]+K*(y-H(:,1:6)*x_h(1:6));
-
- % Correct the navigation states using current perturbation estimates.
- x_h(1:6)=x_h(1:6)+z(1:6);
- x_h(7:10)=Gamma(x_h(7:10),z(7:9));
- delta_u_h=z(10:15);
-
- % Update the Kalman filter state covariance.
- P=(eye(15)-K*H)*P;
-
- % Save the data to the output data structure
- out_data.x_h(:,k)=x_h;
- out_data.diag_P(:,k)=diag(P);
- out_data.delta_u_h(:,k)=delta_u_h;
- out_data.sv(k)=sv;
- out_data.turn(k)=turn;
-end
-
-%% 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
-
-
-
-
-
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%% SUB-FUNCTIONS %%
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%% Init filter %%
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-function [P,Q1,Q2,R,H]=init_filter(settings)
-
-
-% Kalman filter state matrix
-P=zeros(15);
-P(1:3,1:3)=settings.factp(1)^2*eye(3);
-P(4:6,4:6)=settings.factp(2)^2*eye(3);
-P(7:9,7:9)=diag(settings.factp(3:5)).^2;
-P(10:12,10:12)=settings.factp(6)^2*eye(3);
-P(13:15,13:15)=settings.factp(7)^2*eye(3);
-
-% Process noise covariance
-Q1=zeros(6);
-Q1(1:3,1:3)=diag(settings.sigma_acc).^2;
-Q1(4:6,4:6)=diag(settings.sigma_gyro).^2;
-
-Q2=zeros(6);
-Q2(1:3,1:3)=settings.sigma_acc_bias^2*eye(3);
-Q2(4:6,4:6)=settings.sigma_gyro_bias^2*eye(3);
-
-% GNSS-receiver position measurement noise
-R=settings.sigma_gps^2*eye(3);
-
-% Observation matrix
-H=[eye(3) zeros(3,12)];
-
-end
-
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%% Init navigation state %%
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function x_h=init_navigation_state(u,settings)
-
-
-% Calculate the roll and pitch
-f=mean(u(:,1:100),2);
-roll=atan2(-f(2),-f(3));
-pitch=atan2(f(1),norm(f(2:3)));
-
-% Initial coordinate rotation matrix
-Rb2t=Rt2b([roll pitch settings.init_heading])';
-
-% Calculate quaternions
-q=dcm2q(Rb2t);
-
-% Initial navigation state vector
-x_h=[zeros(6,1); q];
-
-end
-
-
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%% State transition matrix %%
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function [F,G]=state_space_model(x,u,Ts)
-
-% Convert quaternion to DCM
-Rb2t=q2dcm(x(7:10));
-
-% Transform measured force to force in
-% the tangent plane coordinate system.
-f_t=Rb2t*u(1:3);
-St=[0 -f_t(3) f_t(2); f_t(3) 0 -f_t(1); -f_t(2) f_t(1) 0];
-
-% Only the standard errors included
-O=zeros(3);
-I=eye(3);
-Fc=[O I O O O;
- O O St Rb2t O;
- O O O O -Rb2t;
- O O O O O;
- O O O O O];
-
-% Approximation of the discret
-% time state transition matrix
-F=eye(15)+Ts*Fc;
-
-% Noise gain matrix
-G=Ts*[O O O O; Rb2t O O O; O -Rb2t O O; O O I O; O O O I];
-end
-
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%% Error correction of quaternion %%
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function q=Gamma(q,epsilon)
-
-% Convert quaternion to DCM
-R=q2dcm(q);
-
-% Construct skew symetric matrx
-OMEGA=[0 -epsilon(3) epsilon(2); epsilon(3) 0 -epsilon(1); -epsilon(2) epsilon(1) 0];
-
-% Correct the DCM matrix
-R=(eye(3)-OMEGA)*R;
-
-% Calculte the corrected quaternions
-q=dcm2q(R);
-end
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%% Random evolution of visible satellites %%
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function sv=random_satellites(sv_old)
-coin=random('uniform',0,1);
-sv=sv_old;
-if sv_old==6
- if coin<0.005
- sv=5;
- end
-end
-if sv_old==5
- if coin>0.99
- sv=6;
- end
- if coin<0.001
- sv=4;
- end
-end
-if sv_old==4
- if coin>0.99
- sv=5;
- end
- if coin<0.0005
- sv=3;
- end
-end
-if sv_old==3
- if coin>0.95
- sv=4;
- end
-end
-end
diff --git a/matlab/GPSaidedINS_cycling.m b/matlab/GPSaidedINS_cycling.m
index 48a7cee..c1bed45 100644
--- a/matlab/GPSaidedINS_cycling.m
+++ b/matlab/GPSaidedINS_cycling.m
@@ -35,12 +35,11 @@ function out_data=GPSaidedINS(in_data,settings)
u=[in_data.IMU.acc;in_data.IMU.gyro];
t=in_data.IMU.t;
-%% Information fusion
+%% Select with part of the cycling data to analyze
+% using all the data takes too much time
start_sim=822799; %start index of time vector of IMU
length_sim=20000;
[is, where]=ismember(in_data.IMU.t(start_sim),in_data.GNSS.t); %find same start on GPS time vector
-ctr_gnss_data=where;
-GPS_position=true;
%% Initialization
% Initialize the navigation state
@@ -61,6 +60,10 @@ out_data.diag_P(:,1)=diag(P);
out_data.delta_u_h=zeros(6,N);
turn_store=true;
+%% Information fusion
+ctr_gnss_data=where;
+GPS_position=true;
+
%% Initialize GPS sensor
sensor.state='warm_start_available';
sensor.fetchfp=1.0;
diff --git a/matlab/gps_randomized.m b/matlab/gps_randomized_car.m
similarity index 99%
rename from matlab/gps_randomized.m
rename to matlab/gps_randomized_car.m
index 4349e72..fa710c9 100644
--- a/matlab/gps_randomized.m
+++ b/matlab/gps_randomized_car.m
@@ -31,8 +31,8 @@ required_sv= 4;
ephDuration=1800.0;
%bounds for time required for fetching freq and phase
fp_lower=0.2;
-fp_upper=500.5;
-
+%fp_upper=500.5;
+fp_upper=0.8;
%% initialize output
sensor = sensor_in;
diff --git a/matlab/gps_randomized_cycling.m b/matlab/gps_randomized_cycling.m
new file mode 100644
index 0000000..fa710c9
--- /dev/null
+++ b/matlab/gps_randomized_cycling.m
@@ -0,0 +1,152 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% 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.2;
+%fp_upper=500.5;
+fp_upper=0.8;
+%% 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.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(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(~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.fetchfp=time+random('uniform',fp_lower,fp_upper);
+ end
+ if(ephemeris_expired)
+ sensor.state='no_info';
+ sensor.eph=0;
+ end
+end
+
+end
+
+
+
+
+
diff --git a/matlab/main_car.m b/matlab/main_car.m
new file mode 100644
index 0000000..6b83dba
--- /dev/null
+++ b/matlab/main_car.m
@@ -0,0 +1,47 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Script for single experiment with car data
+% Plots:
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+%% Load data
+disp('Loads data')
+load('GNSSaidedINS_data.mat');
+
+%% Load filter settings
+disp('Loads settings')
+settings=get_settings_car();
+settings.P_treshold=2.5;
+
+%% Run the GNSS-aided INS
+disp('Runs the GNSS-aided INS')
+out_data=GPSaidedINS(in_data,settings);
+
+%% Plot the data
+disp('Plot data')
+%overall error
+out_data.error
+%plot_data(in_data,out_data,'True');drawnow
+h=zeros(1,2);
+figure(2)
+plot(in_data.GNSS.pos_ned(2,:),in_data.GNSS.pos_ned(1,:),'b-');
+%plot(out_data.x_h(2,:),out_data.x_h(1,:),'r-');
+hold on
+h(1)=plot(in_data.GNSS.pos_ned(2,:),in_data.GNSS.pos_ned(1,:),'b.');
+h(2)=plot(out_data.x_h(2,:),out_data.x_h(1,:),'-');
+%h(3)=plot(in_data.GNSS.pos_ned(2,1),in_data.GNSS.pos_ned(1,1),'ks');
+title('Trajectory')
+ylabel('North [m]')
+xlabel('East [m]')
+legend(h,'GNSS position estimate','GNSS aided INS trajectory')%,'Start point')
+axis equal
+grid on
+
+%plot control signal P and control action turn
+h=zeros(1,2);
+figure(3)
+hold on
+h(1)=plot(out_data.P_store);
+h(2)=plot(out_data.turn);
+legend(h,'sum of the trace of P','antenna turned ON')
+grid on
\ No newline at end of file
diff --git a/matlab/main.m b/matlab/main_cycling.m
similarity index 85%
rename from matlab/main.m
rename to matlab/main_cycling.m
index 9cbfca7..57e3b95 100644
--- a/matlab/main.m
+++ b/matlab/main_cycling.m
@@ -1,7 +1,8 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% Main script for PAN_ powe aware navigation with sensor fusion
+% Script for single experiment with cycling data
+% Plots:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% change input data load and script to be run to switch from car to bike
+
%% Load data
disp('Loads data')
@@ -16,7 +17,6 @@ settings.P_treshold=2.5;
%% Run the GNSS-aided INS
disp('Runs the GNSS-aided INS')
-%out_data=GPSaidedINS(in_data,settings);
out_data=GPSaidedINS_cycling(in_data,settings);
%% Plot the data
--
GitLab