forked from kristianpaul/SoftGNSS
-
Notifications
You must be signed in to change notification settings - Fork 0
/
plotNavigation.m
executable file
·139 lines (119 loc) · 6.33 KB
/
plotNavigation.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
function plotNavigation(navSolutions, settings)
%Functions plots variations of coordinates over time and a 3D position
%plot. It plots receiver coordinates in UTM system or coordinate offsets if
%the true UTM receiver coordinates are provided.
%
%plotNavigation(navSolutions, settings)
%
% Inputs:
% navSolutions - Results from navigation solution function. It
% contains measured pseudoranges and receiver
% coordinates.
% settings - Receiver settings. The true receiver coordinates
% are contained in this structure.
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis
%--------------------------------------------------------------------------
%This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2
%of the License, or (at your option) any later version.
%
%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 General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
%USA.
%--------------------------------------------------------------------------
% CVS record:
% $Id: plotNavigation.m,v 1.1.2.25 2006/08/09 17:20:11 dpl Exp $
%% Plot results in the necessary data exists ==============================
if (~isempty(navSolutions))
%% If reference position is not provided, then set reference position
%% to the average postion
if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ...
|| isnan(settings.truePosition.U)
%=== Compute mean values ==========================================
% Remove NaN-s or the output of the function MEAN will be NaN.
refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E)));
refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N)));
refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U)));
%Also convert geodetic coordinates to deg:min:sec vector format
meanLongitude = dms2mat(deg2dms(...
mean(navSolutions.longitude(~isnan(navSolutions.longitude)))), -5);
meanLatitude = dms2mat(deg2dms(...
mean(navSolutions.latitude(~isnan(navSolutions.latitude)))), -5);
refPointLgText = ['Mean Position\newline Lat: ', ...
num2str(meanLatitude(1)), '{\circ}', ...
num2str(meanLatitude(2)), '{\prime}', ...
num2str(meanLatitude(3)), '{\prime}{\prime}', ...
'\newline Lng: ', ...
num2str(meanLongitude(1)), '{\circ}', ...
num2str(meanLongitude(2)), '{\prime}', ...
num2str(meanLongitude(3)), '{\prime}{\prime}', ...
'\newline Hgt: ', ...
num2str(mean(navSolutions.height(~isnan(navSolutions.height))), '%+6.1f')];
else
refPointLgText = 'Reference Position';
refCoord.E = settings.truePosition.E;
refCoord.N = settings.truePosition.N;
refCoord.U = settings.truePosition.U;
end
figureNumber = 300;
% The 300 is chosen for more convenient handling of the open
% figure windows, when many figures are closed and reopened. Figures
% drawn or opened by the user, will not be "overwritten" by this
% function if the auto numbering is not used.
%=== Select (or create) and clear the figure ==========================
figure(figureNumber);
clf (figureNumber);
set (figureNumber, 'Name', 'Navigation solutions');
%--- Draw axes --------------------------------------------------------
handles(1, 1) = subplot(4, 2, 1 : 4);
handles(3, 1) = subplot(4, 2, [5, 7]);
handles(3, 2) = subplot(4, 2, [6, 8]);
%% Plot all figures =======================================================
%--- Coordinate differences in UTM system -----------------------------
plot(handles(1, 1), [(navSolutions.E - refCoord.E)', ...
(navSolutions.N - refCoord.N)',...
(navSolutions.U - refCoord.U)']);
title (handles(1, 1), 'Coordinates variations in UTM system');
legend(handles(1, 1), 'E', 'N', 'U');
xlabel(handles(1, 1), ['Measurement period: ', ...
num2str(settings.navSolPeriod), 'ms']);
ylabel(handles(1, 1), 'Variations (m)');
grid (handles(1, 1));
axis (handles(1, 1), 'tight');
%--- Position plot in UTM system --------------------------------------
plot3 (handles(3, 1), navSolutions.E - refCoord.E, ...
navSolutions.N - refCoord.N, ...
navSolutions.U - refCoord.U, '+');
hold (handles(3, 1), 'on');
%Plot the reference point
plot3 (handles(3, 1), 0, 0, 0, 'r+', 'LineWidth', 1.5, 'MarkerSize', 10);
hold (handles(3, 1), 'off');
view (handles(3, 1), 0, 90);
axis (handles(3, 1), 'equal');
grid (handles(3, 1), 'minor');
legend(handles(3, 1), 'Measurements', refPointLgText);
title (handles(3, 1), 'Positions in UTM system (3D plot)');
xlabel(handles(3, 1), 'East (m)');
ylabel(handles(3, 1), 'North (m)');
zlabel(handles(3, 1), 'Upping (m)');
%--- Satellite sky plot -----------------------------------------------
skyPlot(handles(3, 2), ...
navSolutions.channel.az, ...
navSolutions.channel.el, ...
navSolutions.channel.PRN(:, 1));
title (handles(3, 2), ['Sky plot (mean PDOP: ', ...
num2str(mean(navSolutions.DOP(2,:))), ')']);
else
disp('plotNavigation: No navigation data to plot.');
end % if (~isempty(navSolutions))