forked from rodralez/NaveGo
-
Notifications
You must be signed in to change notification settings - Fork 0
/
ins_gnss.m
executable file
·418 lines (341 loc) · 14.1 KB
/
ins_gnss.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
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
167
168
169
170
171
172
173
174
175
176
177
178
179
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
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
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
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
function [nav_e] = ins_gnss(imu, gnss, att_mode)
% ins_gnss: loosely-coupled integrated navigation system.
%
% ins_gnss integrates IMU and GNSS measurements by using an Extended Kalman filter.
%
% INPUT
% imu: IMU data structure.
% t: Ix1 time vector (seconds).
% fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
% wb: Ix3 turn rates vector in body frame XYZ (radians/s).
% arw: 1x3 angle random walks (rad/s/root-Hz).
% vrw: 1x3 angle velocity walks (m/s^2/root-Hz).
% gstd: 1x3 gyros standard deviations (radians/s).
% astd: 1x3 accrs standard deviations (m/s^2).
% gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
% ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
% gb_dyn: 1x3 gyros dynamic biases or bias instabilities (radians/s).
% ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
% gb_corr: 1x3 gyros correlation times (seconds).
% ab_corr: 1x3 accrs correlation times (seconds).
% gb_psd: 1x3 gyros dynamic biases PSD (rad/s/root-Hz).
% ab_psd: 1x3 accrs dynamic biases PSD (m/s^2/root-Hz);
% freq: 1x1 sampling frequency (Hz).
% ini_align: 1x3 initial attitude at t(1).
% ini_align_err: 1x3 initial attitude errors at t(1).
%
% gnss: GNSS data structure.
% t: Mx1 time vector (seconds).
% lat: Mx1 latitude (radians).
% lon: Mx1 longitude (radians).
% h: Mx1 altitude (m).
% vel: Mx3 NED velocities (m/s).
% std: 1x3 position standard deviations (rad, rad, m).
% stdm: 1x3 position standard deviations (m, m, m).
% stdv: 1x3 velocity standard deviations (m/s).
% larm: 3x1 lever arm from IMU to GNSS antenna (x-fwd, y-right, z-down) (m).
% freq: 1x1 sampling frequency (Hz).
% zupt_th: 1x1 ZUPT threshold (m/s).
% zupt_win: 1x1 ZUPT time window (seconds).
% eps: 1x1 time interval to compare current IMU time to current GNSS time vector (s).
%
% att_mode: attitude mode string.
% 'quaternion': attitude updated in quaternion format. Default value.
% 'dcm': attitude updated in Direct Cosine Matrix format.
%
% OUTPUT
% nav_e: INS/GNSS navigation estimates data structure.
% t: Ix1 INS time vector (seconds).
% tg: Mx1 GNSS time vector, when Kalman filter was executed (seconds).
% roll: Ix1 roll (radians).
% pitch: Ix1 pitch (radians).
% yaw: Ix1 yaw (radians).
% vel: Ix3 NED velocities (m/s).
% lat: Ix1 latitude (radians).
% lon: Ix1 longitude (radians).
% h: Ix1 altitude (m).
% xi: Mx15 Kalman filter a priori states.
% xp: Mx15 Kalman filter a posteriori states.
% z: Mx6 INS/GNSS measurements
% v: Mx6 Kalman filter innovations.
% b: Mx6 Kalman filter biases compensations, [gb_dyn ab_dyn].
% A: Mx225 Kalman filter transition-state matrices, one matrix per
% row ordered by columns.
% Pp: Mx225 Kalman filter a posteriori covariance matrices, one
% matrix per row ordered by columns.
% Pi: Mx225 Kalman filter a priori covariance matrices, one matrix
% per row ordered by columns.
%
% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
% This file is part of NaveGo, an open-source MATLAB toolbox for
% simulation of integrated navigation systems.
%
% NaveGo is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License (LGPL)
% version 3 as published by the Free Software Foundation.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public
% License along with this program. If not, see
% <http://www.gnu.org/licenses/>.
%
% References:
% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
% simulation framework for low-cost integrated navigation systems,
% Journal of Control Engineering and Applied Informatics, vol. 17,
% issue 2, pp. 110-120, 2015. Alg. 2.
%
% ZUPT algothim based on Paul Groves, Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems (2008). Chapter 13: INS
% Alignment and Zero Velocity Updates.
%
% ins_gps.m, ins_gnss function is based on that previous function.
%
% Version: 005
% Date: 2019/04/19
% Author: Rodrigo Gonzalez <[email protected]>
% URL: https://github.com/rodralez/navego
if nargin < 3, att_mode = 'quaternion'; end
%% ZUPT detection algorithm
zupt = false;
%% PREALLOCATION
% Constant matrices
I = eye(3);
O = zeros(3);
% Length of INS time vector
LI = length(imu.t);
% Length of GNSS time vector
LG = length(gnss.t);
% Attitude
roll_e = zeros (LI, 1);
pitch_e = zeros (LI, 1);
yaw_e = zeros (LI, 1);
% yawm_e = zeros (Mi, 1);
% Initialize estimates at INS time = 1
roll_e(1) = imu.ini_align(1);
pitch_e(1) = imu.ini_align(2);
yaw_e(1) = imu.ini_align(3);
% yawm_e(1) = imu.ini_align(3);
DCMnb = euler2dcm([roll_e(1); pitch_e(1); yaw_e(1);]);
DCMbn = DCMnb';
qua = euler2qua([roll_e(1) pitch_e(1) yaw_e(1)]);
% Velocities
vel_e = zeros (LI, 3);
% Initialize estimates at INS time = 1
vel_e(1,:) = gnss.vel(1,:);
% Positions
lat_e = zeros (LI,1);
lon_e = zeros (LI,1);
h_e = zeros (LI, 1);
% Initialize estimates at INS time = 1
h_e(1) = gnss.h(1);
lat_e(1) = gnss.lat(1);
lon_e(1) = gnss.lon(1);
% Biases
gb_dyn = imu.gb_dyn';
ab_dyn = imu.ab_dyn';
% Initialize Kalman filter matrices
% Prior estimates
kf.xi = [ zeros(1,9), imu.gb_dyn, imu.ab_dyn ]'; % Error vector state
kf.Pi = diag([imu.ini_align_err, gnss.stdv, gnss.std, imu.gb_dyn, imu.ab_dyn].^2);
kf.R = diag([gnss.stdv, gnss.stdm].^2);
kf.Q = diag([imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd].^2);
fb_corrected = (imu.fb(1,:)' + ab_dyn );
fn = (DCMbn * fb_corrected);
% Vector to update matrix F
upd = [gnss.vel(1,:) gnss.lat(1) gnss.h(1) fn'];
% Update matrices F and G
[kf.F, kf.G] = F_update(upd, DCMbn, imu);
[RM,RN] = radius(gnss.lat(1));
Tpr = diag([(RM + gnss.h(1)), (RN + gnss.h(1)) * cos(gnss.lat(1)), -1]); % radians-to-meters
% Update matrix H
kf.H = [ O I O O O ;
O O Tpr O O ; ];
kf.R = diag([gnss.stdv gnss.stdm]).^2;
kf.z = [ gnss.stdv, gnss.stdm ]';
% Propagate prior estimates to get xp(1) and Pp(1)
kf = kf_update( kf );
% PENDING: UD filter matrices
% [Up, Dp] = myUD(S.P);
% dp = diag(Dp);
% DEC = 0.5 * 180/pi; % Magnetic declination (radians)
% Kalman filter matrices for later performance analysis
xi = zeros(LG, 15); % Evolution of Kalman filter a priori states, xi
xp = zeros(LG, 15); % Evolution of Kalman filter a posteriori states, xp
z = zeros(LG, 6); % INS/GNSS measurements
v = zeros(LG, 6); % Kalman filter innovations
b = zeros(LG, 6); % Biases compensantions after Kalman filter correction
A = zeros(LG, 225); % Transition-state matrices, A
Pi = zeros(LG, 225); % Priori covariance matrices, Pi
Pp = zeros(LG, 225); % Posteriori covariance matrices, Pp
K = zeros(LG, 90); % Kalman gain matrices, K
S = zeros(LG, 36); % Innovation matrices, S
% Initialize matrices for Kalman filter performance analysis
xp(1,:) = kf.xp';
Pp(1,:) = reshape(kf.Pp, 1, 225);
b(1,:) = [imu.gb_sta, imu.ab_sta];
% INS (IMU) time is the master clock
for i = 2:LI
%% INERTIAL NAVIGATION SYSTEM (INS)
% Print a dot on console every 10,000 INS executions
if (mod(i,10000) == 0), fprintf('. '); end
% Print a return on console every 200,000 INS executions
if (mod(i,200000) == 0), fprintf('\n'); end
% IMU sampling interval
dti = imu.t(i) - imu.t(i-1);
% Inertial sensors corrected with KF biases estimation
wb_corrected = (imu.wb(i,:)' + gb_dyn );
fb_corrected = (imu.fb(i,:)' + ab_dyn );
% Turn-rates update
omega_ie_n = earthrate(lat_e(i-1));
omega_en_n = transportrate(lat_e(i-1), vel_e(i-1,1), vel_e(i-1,2), h_e(i-1));
% Attitude update
[qua_n, DCMbn, euler] = att_update(wb_corrected, DCMbn, qua, ...
omega_ie_n, omega_en_n, dti, att_mode);
roll_e(i) = euler(1);
pitch_e(i)= euler(2);
yaw_e(i) = euler(3);
qua = qua_n;
% Gravity update
gn = gravity(lat_e(i-1), h_e(i-1));
% Velocity update
fn = (DCMbn * fb_corrected);
vel_n = vel_update(fn, vel_e(i-1,:), omega_ie_n, omega_en_n, gn', dti);
vel_e (i,:) = vel_n;
% Position update
pos = pos_update([lat_e(i-1) lon_e(i-1) h_e(i-1)], vel_e(i,:), dti);
lat_e(i) = pos(1);
lon_e(i) = pos(2);
h_e(i) = pos(3);
% PENDING. Magnetic heading update
% yawm_e(i) = hd_update (imu.mb(i,:), roll_e(i), pitch_e(i), D);
% ZUPT detection algorithm
idz = floor( gnss.zupt_win / dti ); % Index to set ZUPT window time
if ( i > idz )
vel_m = mean (vel_e(i-idz:i , :));
if (abs(vel_m) <= gnss.zupt_th)
% Alternative attitude ZUPT correction
% roll_e(i) = (roll_e(i-idz , :));
% pitch_e(i)= (pitch_e(i-idz , :));
% yaw_e(i) = (yaw_e(i-idz, :));
roll_e(i) = mean (roll_e(i-idz:i , :));
pitch_e(i)= mean (pitch_e(i-idz:i , :));
yaw_e(i) = mean (yaw_e(i-idz:i , :));
lat_e(i) = mean (lat_e(i-idz:i , :));
lon_e(i) = mean (lon_e(i-idz:i , :));
h_e(i) = mean (h_e(i-idz:i , :));
zupt = true;
end
end
%% KALMAN FILTER UPDATE
% Check if there is new GNSS data to process at current INS time
gdx = find (gnss.t >= (imu.t(i) - gnss.eps) & gnss.t < (imu.t(i) + gnss.eps));
if ( ~isempty(gdx) && gdx > 1)
%% INNOVATIONS
[RM,RN] = radius(lat_e(i));
Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]); % radians-to-meters
% Innovations for position with lever arm correction
zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [gnss.lat(gdx); gnss.lon(gdx); gnss.h(gdx);]) ...
+ (DCMbn * gnss.larm);
% Innovations for velocity with lever arm correction
zv = (vel_e(i,:) - gnss.vel(gdx,:) - ((omega_ie_n + omega_en_n) .* (DCMbn * gnss.larm))' ...
+ (DCMbn * skewm(wb_corrected) * gnss.larm )' )';
%% KALMAN FILTER
% GNSS sampling interval
dtg = gnss.t(gdx) - gnss.t(gdx-1);
% Vector to update matrix F
upd = [vel_e(i,:) lat_e(i) h_e(i) fn'];
% Update matrices F and G
[kf.F, kf.G] = F_update(upd, DCMbn, imu);
% Update matrix H
if(zupt == false)
kf.H = [ O I O O O ;
O O Tpr O O ; ];
kf.R = diag([gnss.stdv gnss.stdm]).^2;
kf.z = [ zv' zp' ]';
else
kf.H = [ O I O O O ; ];
kf.R = diag([gnss.stdv]).^2;
kf.z = zv;
end
% Execute the extended Kalman filter
kf.xp(1:9) = 0; % states 1:9 are forced to be zero (error-state approach)
kf = kalman(kf, dtg);
%% INS/GNSS CORRECTIONS
% Quaternion corrections
% Crassidis. Eq. 7.34 and A.174a.
antm = [0 qua_n(3) -qua_n(2); -qua_n(3) 0 qua_n(1); qua_n(2) -qua_n(1) 0];
qua = qua_n + 0.5 .* [qua_n(4)*eye(3) + antm; -1.*[qua_n(1) qua_n(2) qua_n(3)]] * kf.xp(1:3);
qua = qua / norm(qua); % Brute-force normalization
% DCM correction
DCMbn = qua2dcm(qua);
% Another possible attitude correction
% euler = qua2euler(qua);
% roll_e(i) = euler(1);
% pitch_e(i)= euler(2);
% yaw_e(i) = euler(3);
%
% E = skewm(S.xp(1:3));
% DCMbn = (eye(3) + E) * DCMbn_n;
% Attitude corrections
roll_e(i) = roll_e(i) - kf.xp(1);
pitch_e(i) = pitch_e(i) - kf.xp(2);
yaw_e(i) = yaw_e(i) - kf.xp(3);
% Velocity corrections
vel_e (i,1) = vel_e (i,1) - kf.xp(4);
vel_e (i,2) = vel_e (i,2) - kf.xp(5);
vel_e (i,3) = vel_e (i,3) - kf.xp(6);
% Position corrections
lat_e(i) = lat_e(i) - (kf.xp(7));
lon_e(i) = lon_e(i) - (kf.xp(8));
h_e(i) = h_e(i) - kf.xp(9);
% Biases corrections
gb_dyn = kf.xp(10:12);
ab_dyn = kf.xp(13:15);
% Matrices for later Kalman filter performance analysis
xi(gdx,:) = kf.xi';
xp(gdx,:) = kf.xp';
b(gdx,:) = [gb_dyn', ab_dyn'];
A(gdx,:) = reshape(kf.A, 1, 225);
Pi(gdx,:) = reshape(kf.Pi, 1, 225);
Pp(gdx,:) = reshape(kf.Pp, 1, 225);
if(zupt == false)
v(gdx,:) = kf.v';
K(gdx,:) = reshape(kf.K, 1, 90);
S(gdx,:) = reshape(kf.S, 1, 36);
else
zupt = false;
v(gdx,:) = [ kf.v' 0 0 0 ]';
K(gdx,1:45) = reshape(kf.K, 1, 45);
S(gdx,1:9) = reshape(kf.S, 1, 9);
end
end
end
%% Summary from INS/GNSS integration
nav_e.t = imu.t(1:i, :); % INS time vector
nav_e.tg = gnss.t; % GNSS time vector, which is the time vector when the Kalman filter was executed
nav_e.roll = roll_e(1:i, :); % Roll
nav_e.pitch = pitch_e(1:i, :); % Pitch
nav_e.yaw = yaw_e(1:i, :); % Yaw
% nav_e.yawm = yawm_e(1:i, :); % Magnetic heading
nav_e.vel = vel_e(1:i, :); % NED velocities
nav_e.lat = lat_e(1:i, :); % Latitude
nav_e.lon = lon_e(1:i, :); % Longitude
nav_e.h = h_e(1:i, :); % Altitude
nav_e.xi = xi; % A priori states
nav_e.xp = xp; % A posteriori states
nav_e.m = z; % INS/GNSS measurements
nav_e.v = v; % Kalman filter innovations
nav_e.b = b; % Biases compensations
nav_e.A = A; % Transition matrices
nav_e.Pi = Pi; % A priori covariance matrices
nav_e.Pp = Pp; % A posteriori covariance matrices
nav_e.K = K; % Kalman gain matrices
nav_e.S = S; % Innovation matrices
fprintf('\n');
end