-
Notifications
You must be signed in to change notification settings - Fork 47
/
Copy pathGNSS_LS_position_velocity.m
149 lines (114 loc) · 4.9 KB
/
GNSS_LS_position_velocity.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
function [est_r_ea_e,est_v_ea_e,est_clock] = GNSS_LS_position_velocity(...
GNSS_measurements,no_GNSS_meas,predicted_r_ea_e,predicted_v_ea_e)
%GNSS_LS_position_velocity - Calculates position, velocity, clock offset,
%and clock drift using unweighted iterated least squares. Separate
%calculations are implemented for position and clock offset and for
%velocity and clock drift
%
% Software for use with "Principles of GNSS, Inertial, and Multisensor
% Integrated Navigation Systems," Second Edition.
%
% This function created 11/4/2012 by Paul Groves
%
% Inputs:
% GNSS_measurements GNSS measurement data:
% Column 1 Pseudo-range measurements (m)
% Column 2 Pseudo-range rate measurements (m/s)
% Columns 3-5 Satellite ECEF position (m)
% Columns 6-8 Satellite ECEF velocity (m/s)
% no_GNSS_meas Number of satellites for which measurements are
% supplied
% predicted_r_ea_e prior predicted ECEF user position (m)
% predicted_v_ea_e prior predicted ECEF user velocity (m/s)
%
% Outputs:
% est_r_ea_e estimated ECEF user position (m)
% est_v_ea_e estimated ECEF user velocity (m/s)
% est_clock estimated receiver clock offset (m) and drift (m/s)
% Copyright 2012, Paul Groves
% License: BSD; see license.txt for details
% Constants (sone of these could be changed to inputs at a later date)
c = 299792458; % Speed of light in m/s
omega_ie = 7.292115E-5; % Earth rotation rate in rad/s
% Begins
% POSITION AND CLOCK OFFSET
% Setup predicted state
x_pred(1:3,1) = predicted_r_ea_e;
x_pred(4,1) = 0;
test_convergence = 1;
% Repeat until convergence
while test_convergence>0.0001
% Loop measurements
for j = 1:no_GNSS_meas
% Predict approx range
delta_r = GNSS_measurements(j,3:5)' - x_pred(1:3);
approx_range = sqrt(delta_r' * delta_r);
% Calculate frame rotation during signal transit time using (8.36)
C_e_I = [1, omega_ie * approx_range / c, 0;...
-omega_ie * approx_range / c, 1, 0;...
0, 0, 1];
% Predict pseudo-range using (9.143)
delta_r = C_e_I * GNSS_measurements(j,3:5)' - x_pred(1:3);
range = sqrt(delta_r' * delta_r);
pred_meas(j,1) = range + x_pred(4);
% Predict line of sight and deploy in measurement matrix, (9.144)
H_matrix (j,1:3) = - delta_r' / range;
H_matrix (j,4) = 1;
end % for j
% Unweighted least-squares solution, (9.35)/(9.141)
x_est = x_pred + inv(H_matrix(1:no_GNSS_meas,:)' *...
H_matrix(1:no_GNSS_meas,:)) * H_matrix(1:no_GNSS_meas,:)' *...
(GNSS_measurements(1:no_GNSS_meas,1) - pred_meas(1:no_GNSS_meas));
% Test convergence
test_convergence = sqrt((x_est - x_pred)' * (x_est - x_pred));
% Set predictions to estimates for next iteration
x_pred = x_est;
end % while
% Set outputs to estimates
est_r_ea_e(1:3,1) = x_est(1:3);
est_clock(1) = x_est(4);
% VELOCITY AND CLOCK DRIFT
% Skew symmetric matrix of Earth rate
Omega_ie = Skew_symmetric([0,0,omega_ie]);
% Setup predicted state
x_pred(1:3,1) = predicted_v_ea_e;
x_pred(4,1) = 0;
test_convergence = 1;
% Repeat until convergence
while test_convergence>0.0001
% Loop measurements
for j = 1:no_GNSS_meas
% Predict approx range
delta_r = GNSS_measurements(j,3:5)' - est_r_ea_e;
approx_range = sqrt(delta_r' * delta_r);
% Calculate frame rotation during signal transit time using (8.36)
C_e_I = [1, omega_ie * approx_range / c, 0;...
-omega_ie * approx_range / c, 1, 0;...
0, 0, 1];
% Calculate range using (8.35)
delta_r = C_e_I * GNSS_measurements(j,3:5)' - est_r_ea_e;
range = sqrt(delta_r' * delta_r);
% Calculate line of sight using (8.41)
u_as_e = delta_r / range;
% Predict pseudo-range rate using (9.143)
range_rate = u_as_e' * (C_e_I * (GNSS_measurements(j,6:8)' +...
Omega_ie * GNSS_measurements(j,3:5)') - (x_pred(1:3) +...
Omega_ie * est_r_ea_e));
pred_meas(j,1) = range_rate + x_pred(4);
% Predict line of sight and deploy in measurement matrix, (9.144)
H_matrix (j,1:3) = - u_as_e';
H_matrix (j,4) = 1;
end % for j
% Unweighted least-squares solution, (9.35)/(9.141)
x_est = x_pred + inv(H_matrix(1:no_GNSS_meas,:)' *...
H_matrix(1:no_GNSS_meas,:)) * H_matrix(1:no_GNSS_meas,:)' *...
(GNSS_measurements(1:no_GNSS_meas,2) - pred_meas(1:no_GNSS_meas));
% Test convergence
test_convergence = sqrt((x_est - x_pred)' * (x_est - x_pred));
% Set predictions to estimates for next iteration
x_pred = x_est;
end % while
% Set outputs to estimates
est_v_ea_e(1:3,1) = x_est(1:3);
est_clock(2) = x_est(4);
% Ends