Skip to content

Commit 6add7c9

Browse files
max01110max01110
authored
Add files via upload
1 parent 7031a1c commit 6add7c9

File tree

85 files changed

+109397
-0
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

85 files changed

+109397
-0
lines changed

‎EKF/GNSSMaster/Adjust_profile_position.m

+77
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
function Adjust_profile_position(in_filename,out_filename)
2+
%Adjust_profile_position - Adjusts the position in a motion profile file to
3+
% make it consistent with the velocity
4+
%
5+
% Software for use with "Principles of GNSS, Inertial, and Multisensor
6+
% Integrated Navigation Systems," Second Edition.
7+
%
8+
% This function created 2/4/2012 by Paul Groves
9+
%
10+
% Inputs
11+
% in_filename Name of inpit file, e.g. 'In_profile.csv'
12+
% out_filename Name of inpit file, e.g. 'Out_profile.csv'
13+
14+
% Copyright 2012, Paul Groves
15+
% License: BSD; see license.txt for details
16+
17+
% Begins
18+
19+
% Parameters
20+
deg_to_rad = 0.01745329252;
21+
rad_to_deg = 1/deg_to_rad;
22+
23+
24+
% Input truth motion profile from .csv format file
25+
[in_profile,no_epochs,ok] = Read_profile(in_filename);
26+
27+
% End script if there is a problem with the file
28+
if ~ok
29+
return;
30+
end %if
31+
32+
% Initialize navigation solution
33+
old_time = in_profile(1,1);
34+
old_L_b = in_profile(1,2);
35+
old_lambda_b = in_profile(1,3);
36+
old_h_b = in_profile(1,4);
37+
old_v_eb_n = in_profile(1,5:7)';
38+
old_eul_nb = in_profile(1,8:10)';
39+
out_profile(1,:) = in_profile(1,:);
40+
41+
% Main loop
42+
for epoch = 2:no_epochs
43+
44+
% Input data from profile
45+
time = in_profile(epoch,1);
46+
v_eb_n = in_profile(epoch,5:7)';
47+
eul_nb = in_profile(epoch,8:10)';
48+
49+
% Time interval
50+
tor_i = time - old_time;
51+
52+
% Update position
53+
[L_b,lambda_b,h_b]= Update_curvilinear_position(tor_i,old_L_b,...
54+
old_lambda_b,old_h_b,old_v_eb_n,v_eb_n);
55+
56+
% Generate output profile record
57+
out_profile(epoch,1) = time;
58+
out_profile(epoch,2) = L_b;
59+
out_profile(epoch,3) = lambda_b;
60+
out_profile(epoch,4) = h_b;
61+
out_profile(epoch,5:7) = v_eb_n';
62+
out_profile(epoch,8:10) = eul_nb';
63+
64+
% Reset old values
65+
old_time = time;
66+
old_L_b = L_b;
67+
old_lambda_b = lambda_b;
68+
old_h_b = h_b;
69+
old_v_eb_n = v_eb_n;
70+
old_eul_nb = eul_nb;
71+
72+
end %epoch
73+
74+
% Write output profile
75+
Write_profile(out_filename,out_profile);
76+
77+
% Ends

‎EKF/GNSSMaster/Adjust_profile_velocity.m

+79
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
function Adjust_profile_velocity(in_filename,out_filename)
2+
%Adjust_profile_velocity - Adjusts the velocity in a motion profile file to
3+
% make it consistent with the position
4+
%
5+
% Software for use with "Principles of GNSS, Inertial, and Multisensor
6+
% Integrated Navigation Systems," Second Edition.
7+
%
8+
% This function created 2/4/2012 by Paul Groves
9+
%
10+
% Inputs
11+
% in_filename Name of inpit file, e.g. 'In_profile.csv'
12+
% out_filename Name of inpit file, e.g. 'Out_profile.csv'
13+
14+
% Copyright 2012, Paul Groves
15+
% License: BSD; see license.txt for details
16+
17+
% Begins
18+
19+
% Parameters
20+
deg_to_rad = 0.01745329252;
21+
rad_to_deg = 1/deg_to_rad;
22+
23+
24+
% Input truth motion profile from .csv format file
25+
[in_profile,no_epochs,ok] = Read_profile(in_filename);
26+
27+
% End script if there is a problem with the file
28+
if ~ok
29+
return;
30+
end %if
31+
32+
% Initialize navigation solution
33+
old_time = in_profile(1,1);
34+
old_L_b = in_profile(1,2);
35+
old_lambda_b = in_profile(1,3);
36+
old_h_b = in_profile(1,4);
37+
old_v_eb_n = in_profile(1,5:7)';
38+
old_eul_nb = in_profile(1,8:10)';
39+
out_profile(1,:) = in_profile(1,:);
40+
41+
% Main loop
42+
for epoch = 2:no_epochs
43+
44+
% Input data from profile
45+
time = in_profile(epoch,1);
46+
L_b = in_profile(epoch,2);
47+
lambda_b = in_profile(epoch,3);
48+
h_b = in_profile(epoch,4);
49+
eul_nb = in_profile(epoch,8:10)';
50+
51+
% Time interval
52+
tor_i = time - old_time;
53+
54+
% Update velocity
55+
v_eb_n = Velocity_from_curvilinear(tor_i,old_L_b,...
56+
old_lambda_b,old_h_b,old_v_eb_n,L_b,lambda_b,h_b);
57+
58+
% Generate output profile record
59+
out_profile(epoch,1) = time;
60+
out_profile(epoch,2) = L_b;
61+
out_profile(epoch,3) = lambda_b;
62+
out_profile(epoch,4) = h_b;
63+
out_profile(epoch,5:7) = v_eb_n';
64+
out_profile(epoch,8:10) = eul_nb';
65+
66+
% Reset old values
67+
old_time = time;
68+
old_L_b = L_b;
69+
old_lambda_b = lambda_b;
70+
old_h_b = h_b;
71+
old_v_eb_n = v_eb_n;
72+
old_eul_nb = eul_nb;
73+
74+
end %epoch
75+
76+
% Write output profile
77+
Write_profile(out_filename,out_profile);
78+
79+
% Ends

‎EKF/GNSSMaster/CTM_to_Euler.m

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
function eul = CTM_to_Euler(C)
2+
%CTM_to_Euler - Converts a coordinate transformation matrix to the
3+
%corresponding set of Euler angles%
4+
%
5+
% Software for use with "Principles of GNSS, Inertial, and Multisensor
6+
% Integrated Navigation Systems," Second Edition.
7+
%
8+
% This function created 1/4/2012 by Paul Groves
9+
%
10+
% Inputs:
11+
% C coordinate transformation matrix describing transformation from
12+
% beta to alpha
13+
%
14+
% Outputs:
15+
% eul Euler angles describing rotation from beta to alpha in the
16+
% order roll, pitch, yaw(rad)
17+
18+
% Copyright 2012, Paul Groves
19+
% License: BSD; see license.txt for details
20+
21+
% Begins
22+
23+
% Calculate Euler angles using (2.23)
24+
eul(1,1) = atan2(C(2,3),C(3,3)); % roll
25+
eul(2,1) = - asin(C(1,3)); % pitch
26+
eul(3,1) = atan2(C(1,2),C(1,1)); % yaw
27+
28+
% Ends

‎EKF/GNSSMaster/Calculate_errors_NED.m

+51
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
function [delta_r_eb_n,delta_v_eb_n,delta_eul_nb_n] = Calculate_errors_NED(...
2+
est_L_b,est_lambda_b,est_h_b,est_v_eb_n,est_C_b_n,true_L_b,...
3+
true_lambda_b,true_h_b,true_v_eb_n,true_C_b_n)
4+
%Calculate_errors_NED - Calculates the position, velocity, and attitude
5+
% errors of a NED navigation solution.
6+
%
7+
% Software for use with "Principles of GNSS, Inertial, and Multisensor
8+
% Integrated Navigation Systems," Second Edition.
9+
%
10+
% This function created 3/4/2012 by Paul Groves
11+
%
12+
% Inputs:
13+
% est_L_b latitude solution (rad)
14+
% est_lambda_b longitude solution (rad)
15+
% est_h_b height solution (m)
16+
% est_v_eb_n velocity solution of body frame w.r.t. ECEF frame,
17+
% resolved along north, east, and down (m/s)
18+
% est_C_b_n body-to-NED coordinate transformation matrix solution
19+
% true_L_b true latitude (rad)
20+
% true_lambda_b true longitude (rad)
21+
% true_h_b true height (m)
22+
% true_v_eb_n true velocity of body frame w.r.t. ECEF frame, resolved
23+
% along north, east, and down (m/s)
24+
% C_b_n true body-to-NED coordinate transformation matrix
25+
%
26+
% Outputs:
27+
% delta_r_eb_n position error resolved along NED (m)
28+
% delta_v_eb_n velocity error resolved along NED (m/s)
29+
% delta_eul_nb_n attitude error as NED Euler angles (rad)
30+
% These are expressed about north, east, and down
31+
32+
% Copyright 2012, Paul Groves
33+
% License: BSD; see license.txt for details
34+
35+
% Begins
36+
37+
% Position error calculation, using (2.119)
38+
[R_N,R_E] = Radii_of_curvature(true_L_b);
39+
delta_r_eb_n(1) = (est_L_b - true_L_b) * (R_N + true_h_b);
40+
delta_r_eb_n(2) = (est_lambda_b - true_lambda_b) * (R_E + true_h_b) *...
41+
cos(true_L_b);
42+
delta_r_eb_n(3) = -(est_h_b - true_h_b);
43+
44+
% Velocity error calculation
45+
delta_v_eb_n = est_v_eb_n - true_v_eb_n;
46+
47+
% Attitude error calculation, using (5.109) and (5.111)
48+
delta_C_b_n = est_C_b_n * true_C_b_n';
49+
delta_eul_nb_n = -CTM_to_Euler(delta_C_b_n);
50+
51+
% Ends

‎EKF/GNSSMaster/ECEF_to_ECI.m

+47
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
function [r_ib_i,v_ib_i,C_b_i] = ECEF_to_ECI(t,r_eb_e,v_eb_e,C_b_e)
2+
%ECEF_to_ECI - Converts position, velocity, and attitude from ECEF- to
3+
%ECI-frame referenced and resolved
4+
%地心地固坐标系转换到地心惯性坐标系
5+
% Software for use with "Principles of GNSS, Inertial, and Multisensor
6+
% Integrated Navigation Systems," Second Edition.
7+
%
8+
% This function created 2/4/2012 by Paul Groves
9+
%
10+
% Inputs:
11+
% t time (s)
12+
% r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
13+
% along ECEF-frame axes (m)
14+
% v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
15+
% ECEF-frame axes (m/s)
16+
% C_b_e body-to-ECEF-frame coordinate transformation matrix
17+
%
18+
% Outputs:
19+
% r_ib_i Cartesian position of body frame w.r.t. ECI frame, resolved
20+
% along ECI-frame axes (m)
21+
% v_ib_i velocity of body frame w.r.t. ECI frame, resolved along
22+
% ECI-frame axes (m/s)
23+
% C_b_i body-to-ECI-frame coordinate transformation matrix
24+
25+
% Parameters
26+
omega_ie = 7.292115E-5; % Earth rotation rate (rad/s)
27+
28+
% Copyright 2012, Paul Groves
29+
% License: BSD; see license.txt for details
30+
31+
% Begins
32+
33+
% Calculate ECEF to ECI coordinate transformation matrix using (2.145)
34+
C_e_i = [cos(omega_ie * t), -sin(omega_ie * t), 0;...
35+
sin(omega_ie * t), cos(omega_ie * t), 0;...
36+
0, 0, 1];
37+
38+
% Transform position using (2.146)
39+
r_ib_i = C_e_i * r_eb_e;
40+
41+
% Transform velocity using (2.145)
42+
v_ib_i = C_e_i * (v_eb_e + omega_ie * [-r_eb_e(2);r_eb_e(1);0]);
43+
44+
% Transform attitude using (2.15)
45+
C_b_i = C_e_i * C_b_e;
46+
47+
% Ends

‎EKF/GNSSMaster/ECEF_to_NED.m

+100
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
function [L_b,lambda_b,h_b,v_eb_n,C_b_n,C_e_n,omega_en,C_ni] = ECEF_to_NED(r_eb_e,v_eb_e,C_b_e,t)
2+
%ECEF_to_NED - Converts Cartesian to curvilinear position, velocity
3+
%resolving axes from ECEF to NED and attitude from ECEF- to NED-referenced
4+
%
5+
% Software for use with "Principles of GNSS, Inertial, and Multisensor
6+
% Integrated Navigation Systems," Second Edition.
7+
%
8+
% This function created 2/4/2012 by Paul Groves
9+
%
10+
% Inputs:
11+
% r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
12+
% along ECEF-frame axes (m)
13+
% v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
14+
% ECEF-frame axes (m/s)
15+
% C_b_e body-to-ECEF-frame coordinate transformation matrix
16+
%
17+
% Outputs:
18+
% L_b latitude (rad)
19+
% lambda_b longitude (rad)
20+
% h_b height (m)
21+
% v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
22+
% north, east, and down (m/s)
23+
% C_b_n body-to-NED coordinate transformation matrix
24+
25+
% Parameters
26+
R_0 = 6378137; %WGS84 Equatorial radius in meters
27+
e = 0.0818191908425; %WGS84 eccentricity
28+
29+
% Copyright 2012, Paul Groves
30+
% License: BSD; see license.txt for details
31+
32+
% Begins
33+
34+
% Convert position using Borkowski closed-form exact solution
35+
% From (2.113)
36+
lambda_b = atan2(r_eb_e(2),r_eb_e(1));
37+
38+
% From (C.29) and (C.30)
39+
k1 = sqrt(1 - e^2) * abs (r_eb_e(3));
40+
k2 = e^2 * R_0;
41+
beta = sqrt(r_eb_e(1)^2 + r_eb_e(2)^2);
42+
E = (k1 - k2) / beta;
43+
F = (k1 + k2) / beta;
44+
45+
% From (C.31)
46+
P = 4/3 * (E*F + 1);
47+
48+
% From (C.32)
49+
Q = 2 * (E^2 - F^2);
50+
51+
% From (C.33)
52+
D = P^3 + Q^2;
53+
54+
% From (C.34)
55+
V = (sqrt(D) - Q)^(1/3) - (sqrt(D) + Q)^(1/3);
56+
57+
% From (C.35)
58+
G = 0.5 * (sqrt(E^2 + V) + E);
59+
60+
% From (C.36)
61+
T = sqrt(G^2 + (F - V * G) / (2 * G - E)) - G;
62+
63+
% From (C.37)
64+
L_b = sign(r_eb_e(3)) * atan((1 - T^2) / (2 * T * sqrt (1 - e^2)));
65+
66+
% From (C.38)
67+
h_b = (beta - R_0 * T) * cos(L_b) +...
68+
(r_eb_e(3) - sign(r_eb_e(3)) * R_0 * sqrt(1 - e^2)) * sin (L_b);
69+
70+
% Calculate ECEF to NED coordinate transformation matrix using (2.150)
71+
cos_lat = cos(L_b);
72+
sin_lat = sin(L_b);
73+
cos_long = cos(lambda_b);
74+
sin_long = sin(lambda_b);
75+
C_e_n = [-sin_lat * cos_long, -sin_lat * sin_long, cos_lat;...
76+
-sin_long, cos_long, 0;...
77+
-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat];
78+
79+
% Transform velocity using (2.73)
80+
v_eb_n = C_e_n * v_eb_e;
81+
82+
% Transform attitude using (2.15)
83+
C_b_n = C_e_n * C_b_e;
84+
85+
% Add stuff for angular velocity calculations
86+
% Angular velocity of NED frame with respect to ECEF frame
87+
R_N = (R_0*(1-e^2))/((1-(e^2)*sin(L_b)^2)^(3/2));
88+
R_E = R_0/(sqrt(1-(e^2)*sin(L_b)^2));
89+
90+
omega_en = [v_eb_n(2)/(R_E + h_b);...
91+
-v_eb_n(1)/(R_N + h_b);...
92+
(-v_eb_n(2)*tan(L_b))/(R_E + h_b)]; %Eq 5.44
93+
94+
% Compute rotation matrix forom Inertial frame ECI to NED
95+
omega_ie = 7.292115E-5; % Earth rotation rate (rad/s)
96+
C_ni = [-sin(L_b)*cos(lambda_b+omega_ie*t), -sin(L_b)*sin(lambda_b +omega_ie*t), cos(L_b);...
97+
-sin(lambda_b+omega_ie*t), cos(lambda_b+omega_ie*t),0;...
98+
-cos(L_b)*cos(lambda_b+omega_ie*t), -cos(L_b)*sin(lambda_b+omega_ie*t), -sin(L_b)];
99+
100+
% Ends

0 commit comments

Comments
 (0)