diff --git a/matlab/GPSaidedINS.m b/matlab/GPSaidedINS.m
index 50013b28ecab6f23a12de30497d7c17a790915d7..75c96af77f12bd667d1b3928ee2ae9fc501173ac 100644
--- a/matlab/GPSaidedINS.m
+++ b/matlab/GPSaidedINS.m
@@ -53,6 +53,7 @@ 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;
@@ -84,8 +85,8 @@ for k=2:N
%% GPS sensor %%
%%%%%%%%%%%%%%%%%
%randomized number of visible satellites
- sv = random_satellites(sv);
- %sv=5;
+ %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
@@ -164,6 +165,7 @@ for k=2:N
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
diff --git a/matlab/GPSaidedINS_cycling.m b/matlab/GPSaidedINS_cycling.m
index 320ebfc03c08c8c59481326c0e970429f32c7799..9461e86a61baa6fea50ac7e393ddee71644158bd 100644
--- a/matlab/GPSaidedINS_cycling.m
+++ b/matlab/GPSaidedINS_cycling.m
@@ -37,7 +37,7 @@ t=in_data.IMU.t;
%% Initialization
% Initialize the navigation state
-x_h=init_navigation_state(u,settings);
+x_h=init_navigation_state(u,settings,in_data);
% Initialize the sensor bias estimate
delta_u_h=zeros(6,1);
@@ -53,6 +53,7 @@ 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);
+turn_store=true;
%% Information fusion
ctr_gnss_data=1838;
@@ -76,10 +77,10 @@ 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
-for k=200000:210000
+for k=200000:209000
% Sampling period
- Ts=t(k)-t(k-1);
+ Ts=0.01;%t(k)-t(k-1);
%%%%%%%%%%%%%%%%%
%% GPS sensor %%
@@ -96,9 +97,9 @@ for k=200000:210000
end
%update sensor state
% pre_state=sensor.state;
- sensor=gps_randomized(t(k),sensor,false,sv); %remove _randomized for worst case
+ sensor=gps_randomized(t(k),sensor,turn,sv); %remove _randomized for worst case
%determine if GPS position is available
- GPS_position=true;%strcmp(sensor.state,'position_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;
@@ -165,6 +166,7 @@ for k=200000:210000
out_data.diag_P(:,k)=diag(P);
out_data.delta_u_h(:,k)=delta_u_h;
out_data.sv(k)=sv;
+ turn_store=[turn_store, turn];
end
%% plot feedback over P matrix
@@ -180,7 +182,8 @@ ygps = interp1(in_data.GNSS.t,in_data.GNSS.pos_ned(1,:),in_data.IMU.t,'linear','
xerr = xest - xgps;
yerr = yest - ygps;
out_data.error = sqrt(mean(xerr.^2+yerr.^2));
-out_data.P_store=P_store;
+out_data.P=P_store;
+out_data.turn=turn_store;
end
@@ -228,7 +231,7 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Init navigation state %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function x_h=init_navigation_state(u,settings)
+function x_h=init_navigation_state(u,settings,in_data)
% Calculate the roll and pitch
@@ -243,7 +246,8 @@ Rb2t=Rt2b([roll pitch settings.init_heading])';
q=dcm2q(Rb2t);
% Initial navigation state vector
-x_h=[zeros(6,1); q];
+x_h=[in_data.GNSS.pos_ned(:,1838);zeros(3,1); q];
+%x_h=[zeros(6,1); q];
end
diff --git a/matlab/cycling_data_final.mat b/matlab/cycling_data_final.mat
new file mode 100644
index 0000000000000000000000000000000000000000..d7f184af4e29fcf9af9ae56edb47ef0b94afff78
Binary files /dev/null and b/matlab/cycling_data_final.mat differ
diff --git a/matlab/get_settings.m b/matlab/get_settings.m
index f4946dd13c4a7cfbeb180a9eb158b2099c7f1bc3..193930b7eb2a2f56a6c41101386af5d5dd59b6c8 100644
--- a/matlab/get_settings.m
+++ b/matlab/get_settings.m
@@ -25,8 +25,8 @@ settings.init_heading = 320*pi/180;
% Process noise covariance (Q)
% Standard deviations, need to be squared
-settings.sigma_acc = 0.05; % [m/s^2]
-settings.sigma_gyro = 0.1*pi/180; % [rad/s]
+settings.sigma_acc = 0.50;%0.05; % [m/s^2]
+settings.sigma_gyro = 1.0*pi/180;%0.1*pi/180; % [rad/s]
settings.sigma_acc_bias = 0.0001; % [m/s^2.5]
settings.sigma_gyro_bias = 0.01*pi/180; % [rad/s^1.5]
diff --git a/matlab/gps_randomized.m b/matlab/gps_randomized.m
index 9db2d121adc24f677c3dab5f9a665db996d7e15f..cd274e7d9d61a726b584291abc7779c4ac30edd8 100644
--- a/matlab/gps_randomized.m
+++ b/matlab/gps_randomized.m
@@ -31,7 +31,7 @@ required_sv= 4;
ephDuration=1800.0;
%bounds for time required for fetching freq and phase
fp_lower=0.2;
-fp_upper=1.5;
+fp_upper=500.5;
%% initialize output
sensor = sensor_in;
@@ -106,16 +106,16 @@ if strcmp(sensor_in.state,'read_ephemeris')
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(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);
@@ -137,10 +137,10 @@ if strcmp(sensor_in.state,'cold_start')
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(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;
diff --git a/matlab/main.m b/matlab/main.m
index b65b0b558dc6f5139746f0635830951e66448876..cac6874613a59367f778da7bc3e26cad40a3fb9e 100644
--- a/matlab/main.m
+++ b/matlab/main.m
@@ -11,7 +11,7 @@ load('cycling_input_data');
%% Load filter settings
disp('Loads settings')
settings=get_settings();
-settings.P_treshold=0.1;
+settings.P_treshold=6.1;
%% Run the GNSS-aided INS
disp('Runs the GNSS-aided INS')
@@ -21,4 +21,21 @@ out_data=GPSaidedINS_cycling(in_data,settings);
%% Plot the data
disp('Plot data')
%plot_data(in_data,out_data,'True');drawnow
+h=zeros(1,3);
+figure(2)
+plot(in_data.GNSS.pos_ned(2,:),in_data.GNSS.pos_ned(1,:),'b-');
+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,:),'r');
+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
+figure(3)
+hold on
+plot(out_data.P)
+plot(out_data.turn)
\ No newline at end of file
diff --git a/matlab/run_many.m b/matlab/run_many.m
index 5f3b55509744c2509506cd4c0ac783a9931f5170..5b10dbdb714c72fcd9a113a7fa11dd71a7f9d699 100644
--- a/matlab/run_many.m
+++ b/matlab/run_many.m
@@ -18,6 +18,7 @@ settings.P_treshold=0.1;
figure(4)
hold on
+grid
color=[[0.7,0.0,0.0];[0.0,0.7,0.0];[0.7,0.0,0.7];[0.0,0.0,0.7]];
j=1;
for P_treshold=[2.0,4.0,6.0,8.0]%[2.3,2.8,3.3,3.8,4.3,4.8,5.3,5.8,6.3,6.8,7.3]
diff --git a/matlab/run_many_cycling.m b/matlab/run_many_cycling.m
new file mode 100644
index 0000000000000000000000000000000000000000..2ebf28d6c23c7e7744cff24aaee1e54db865bd19
--- /dev/null
+++ b/matlab/run_many_cycling.m
@@ -0,0 +1,56 @@
+%"monteCarlo" example
+
+%% Load data
+disp('Loads data')
+load('cycling_input_data');
+
+%% Load filter settings
+disp('Loads settings')
+settings=get_settings();
+
+
+settings.P_treshold=0.1;
+% %run first simulation
+% out_data=GPSaidedINS(in_data,settings);
+% energy=out_data.energy;
+% error=out_data.error;
+
+
+figure(5)
+hold on
+grid
+color=[[0.7,0.0,0.0];[0.0,0.7,0.0];[0.7,0.0,0.7];[0.0,0.0,0.7]];
+j=1;
+for P_treshold=[5.0,7.0,9.0,11.0]%[2.3,2.8,3.3,3.8,4.3,4.8,5.3,5.8,6.3,6.8,7.3]
+ clear energy error
+ settings.P_treshold=P_treshold;
+ %run first simulation
+ out_data=GPSaidedINS_cycling(in_data,settings);
+ energy=out_data.energy;
+ error=out_data.error;
+% energy=[energy,out_data.energy];
+% error=[error,out_data.error];
+ %run more simulations
+ for k=1:10
+ %% Run the GNSS-aided INS
+ disp('Running the GNSS-aided INS')
+ P_treshold
+ k
+ out_data=GPSaidedINS_cycling(in_data,settings);
+ energy=[energy,out_data.energy];
+ error=[error,out_data.error];
+ end
+ %plot data
+ scatter(energy, error,40,color(j,:),'o','filled')
+ xlabel('energy')
+ ylabel('error')
+ %title(['P treshold = ', num2str(P_treshold)])
+
+ j=j+1;
+end
+% %plot data
+% figure(2)
+% scatter(energy, error)
+% xlabel('energy')
+% ylabel('error')
+% grid