GPSaidedINS_cycling.m 9.22 KB
Newer Older
Claudio Mandrioli's avatar
Claudio Mandrioli committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% 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.
Claudio Mandrioli's avatar
Claudio Mandrioli committed
23
% out_data.energy           overall estimated power consumption
Claudio Mandrioli's avatar
Claudio Mandrioli committed
24
25
26
27
28
% out_data.error            average RMS
%
%
% Edit: Isaac Skog (skog@kth.se), 2016-09-06
% Revised: Bo Bernhardsson, 2018-01-01
29
% Revised: Claudio Mandrioli 2018-02-26
Claudio Mandrioli's avatar
Claudio Mandrioli committed
30
31
32
33
34
35
36
37
38
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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;

39
40
%% Select with part of the cycling data to analyze
% using all the data takes too much time
Claudio Mandrioli's avatar
Claudio Mandrioli committed
41
start_sim=822799;               %start index of time vector of IMU
Claudio Mandrioli's avatar
Claudio Mandrioli committed
42
length_sim=20000;
Claudio Mandrioli's avatar
Claudio Mandrioli committed
43
44
[is, where]=ismember(in_data.IMU.t(start_sim),in_data.GNSS.t); %find same start on GPS time vector

Claudio Mandrioli's avatar
Claudio Mandrioli committed
45
46
%% Initialization
% Initialize the navigation state
Claudio Mandrioli's avatar
Claudio Mandrioli committed
47
x_h=init_navigation_state(u,settings,in_data,where);
Claudio Mandrioli's avatar
Claudio Mandrioli committed
48
49
50
51
52
53
54
55
56
57
58
59
60
61

% 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);
Claudio Mandrioli's avatar
Claudio Mandrioli committed
62
turn_store=true;
Claudio Mandrioli's avatar
Claudio Mandrioli committed
63

64
65
66
67
%% Information fusion
ctr_gnss_data=where;
GPS_position=true;

Claudio Mandrioli's avatar
Claudio Mandrioli committed
68
69
70
71
%% Initialize GPS sensor 
sensor.state='warm_start_available';
sensor.fetchfp=1.0;
sensor.geteph=Inf;
Claudio Mandrioli's avatar
Claudio Mandrioli committed
72
sensor.ephExp=in_data.IMU.t(start_sim)+180000.0;
Claudio Mandrioli's avatar
Claudio Mandrioli committed
73
74
75
76
77
78
79
80
81
82
83
84
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

Claudio Mandrioli's avatar
Claudio Mandrioli committed
85
for k=start_sim:start_sim+length_sim
Claudio Mandrioli's avatar
Claudio Mandrioli committed
86
87

    % Sampling period
Claudio Mandrioli's avatar
Claudio Mandrioli committed
88
    Ts=0.01;%t(k)-t(k-1);
Claudio Mandrioli's avatar
Claudio Mandrioli committed
89
90
91
92
93
    
    %%%%%%%%%%%%%%%%%
    %% GPS sensor  %%
    %%%%%%%%%%%%%%%%%
    %randomized number of visible satellites
94
    sv = random_satellites(sv);
95
    %sv=5;                               %uncomment for simulations without loss of visibility 
Claudio Mandrioli's avatar
Claudio Mandrioli committed
96
97
98
99
100
101
102
103
    %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
104
    sensor=gps_randomized_cycling(t(k),sensor,turn,sv);
Claudio Mandrioli's avatar
Claudio Mandrioli committed
105
    %determine if GPS position is available
Claudio Mandrioli's avatar
Claudio Mandrioli committed
106
    GPS_position=strcmp(sensor.state,'position_available');
Claudio Mandrioli's avatar
Claudio Mandrioli committed
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
    %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;
167
    out_data.turn(k-start_sim+1)=turn;
Claudio Mandrioli's avatar
Claudio Mandrioli committed
168
169
170
171
end

%% output for simulation of energy-accuracy tradeoff 
out_data.energy=energy;
172
173
xest = out_data.x_h(2,start_sim:start_sim+length_sim);
yest = out_data.x_h(1,start_sim:start_sim+length_sim);
Claudio Mandrioli's avatar
Claudio Mandrioli committed
174
175
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')';
Claudio Mandrioli's avatar
Claudio Mandrioli committed
176
177
178
xerr = xest - xgps; 
yerr = yest - ygps;
out_data.error = sqrt(mean(xerr.^2+yerr.^2));
Claudio Mandrioli's avatar
Claudio Mandrioli committed
179
out_data.P_store=P_store;
Claudio Mandrioli's avatar
Claudio Mandrioli committed
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
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     %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Claudio Mandrioli's avatar
Claudio Mandrioli committed
227
function x_h=init_navigation_state(u,settings,in_data,where)
Claudio Mandrioli's avatar
Claudio Mandrioli committed
228
229
230
231
232
233
234
235
236
237
238
239
240
241


% 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
Claudio Mandrioli's avatar
Claudio Mandrioli committed
242
x_h=[in_data.GNSS.pos_ned(:,where);zeros(3,1); q];
Claudio Mandrioli's avatar
Claudio Mandrioli committed
243
%x_h=[zeros(6,1); q];
Claudio Mandrioli's avatar
Claudio Mandrioli committed
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303

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;
Claudio Mandrioli's avatar
Claudio Mandrioli committed
304
if sv_old==6 
Claudio Mandrioli's avatar
Claudio Mandrioli committed
305
306
307
308
    if coin<0.005
        sv=5;
    end
end
Claudio Mandrioli's avatar
Claudio Mandrioli committed
309
if sv_old==5
Claudio Mandrioli's avatar
Claudio Mandrioli committed
310
311
312
313
314
315
316
    if coin>0.99
        sv=6;
    end
    if coin<0.001
        sv=4;
    end     
end
Claudio Mandrioli's avatar
Claudio Mandrioli committed
317
if sv_old==4  
Claudio Mandrioli's avatar
Claudio Mandrioli committed
318
319
320
321
322
323
324
    if coin>0.99
        sv=5;
    end
    if coin<0.0005
        sv=3;
    end     
end
Claudio Mandrioli's avatar
Claudio Mandrioli committed
325
if sv_old==3
Claudio Mandrioli's avatar
Claudio Mandrioli committed
326
327
328
329
330
    if coin>0.95
        sv=4;
    end
end
end