Select Git revision
GPSaidedINS_cycling.m
Claudio Mandrioli authored
GPSaidedINS_cycling.m 9.22 KiB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% 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
% Revised: Claudio Mandrioli 2018-02-26
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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;
%% 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
%% Initialization
% Initialize the navigation state
x_h=init_navigation_state(u,settings,in_data,where);
% 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);
turn_store=true;
%% Information fusion
ctr_gnss_data=where;
GPS_position=true;
%% Initialize GPS sensor
sensor.state='warm_start_available';
sensor.fetchfp=1.0;sensor.geteph=Inf;
sensor.ephExp=in_data.IMU.t(start_sim)+180000.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=start_sim:start_sim+length_sim
% Sampling period
Ts=0.01;%t(k)-t(k-1);
%%%%%%%%%%%%%%%%%
%% GPS sensor %%
%%%%%%%%%%%%%%%%%
%randomized number of visible satellites
sv = random_satellites(sv);
%sv=5; %uncomment for simulations without loss of visibility
%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
sensor=gps_randomized_cycling(t(k),sensor,turn,sv);
%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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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-start_sim+1)=turn;
end
%% output for simulation of energy-accuracy tradeoff
out_data.energy=energy;
xest = out_data.x_h(2,start_sim:start_sim+length_sim);
yest = out_data.x_h(1,start_sim:start_sim+length_sim);
xgps = interp1(in_data.GNSS.t,in_data.GNSS.pos_ned(2,:),in_data.IMU.t(start_sim:start_sim+length_sim),'linear','extrap')';
ygps = interp1(in_data.GNSS.t,in_data.GNSS.pos_ned(1,:),in_data.IMU.t(start_sim:start_sim+length_sim),'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,in_data,where)
% 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=[in_data.GNSS.pos_ned(:,where);zeros(3,1); q];
%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