Skip to content

GitLab

Commit fb78a80e authored by Claudio Mandrioli's avatar Claudio Mandrioli
Browse files

divided main files and gps model files for car and cycling data

parent 9fdf3c45
Hide whitespace changes
Inline Side-by-side
matlab/GPSaidedINS.m deleted 100644 → 0
View file @ 9fdf3c45
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% 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
matlab/GPSaidedINS_cycling.m
View file @ fb78a80e
......@@ -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;
......
matlab/gps_randomized.m → matlab/gps_randomized_car.m
View file @ fb78a80e
......@@ -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;
......
matlab/gps_randomized_cycling.m 0 → 100644
View file @ fb78a80e
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 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
matlab/main_car.m 0 → 100644
View file @ fb78a80e
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
matlab/main.m → matlab/main_cycling.m
View file @ fb78a80e
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment