Appendix A. Listing — различия между версиями
Материал из SRNS
Korogodin (обсуждение | вклад) |
Korogodin (обсуждение | вклад) м (переименовал «Тестовая страница» в «Appendix A. Listing») |
||
(не показаны 19 промежуточных версий 2 участников) | |||
Строка 1: | Строка 1: | ||
− | + | __NOTOC__ | |
− | + | === main.m === | |
− | + | <source lang="matlab">try | |
− | < | + | close(handle_fig_main); % Close old output form |
+ | end | ||
− | + | close all | |
+ | clear | ||
+ | clc | ||
− | + | globals; | |
− | + | addpath([pwd '/func/tracking']); % Functions for tracking algorithm | |
+ | addpath([pwd '/func/solve']); % Functions for solving without noise | ||
+ | addpath([pwd '/func/interface']); % Functions for interface | ||
− | [ | + | Tmod = 3.2*60*60; %[s], duration of the simulation |
− | [[ | + | % Magic constants |
− | [[ | + | hF_cont = 0; % Last figure's handles |
+ | Font_Size = 8; % Font size for output interface | ||
+ | mu_earth = 3.9860044e14; % [m^3/s^2] Gravity constant | ||
+ | omega_e = 0.7292115e-4; % [rad/s] Earth's rotation rate | ||
+ | options_solve = optimset('Display','off'); % Turn off display for fsolve | ||
+ | |||
+ | % Load true trajectory of SV | ||
+ | load TrueTrajectory.mat | ||
+ | Nmod = fix(Tmod/T); | ||
+ | if Nmod > Nmod_max | ||
+ | Nmod = Nmod_max; | ||
+ | Tmod = (Nmod-1)*T; | ||
+ | end | ||
+ | resize_arrays; % resize arrays for new Tmod | ||
+ | |||
+ | handle_fig_main = fig_main(); % open GUI form</source> | ||
+ | |||
+ | |||
+ | ===globals.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | |||
+ | global Font_Size mu_earth omega_e | ||
+ | |||
+ | global SV_GLO_List SV_GLO | ||
+ | |||
+ | global Xist Xextr Xest Xest_won | ||
+ | |||
+ | global Tmod dTmod tmod Nmod | ||
+ | |||
+ | global start_handle hF_cont | ||
+ | |||
+ | global options_solve | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===fig_main.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | function varargout = fig_main(varargin) | ||
+ | % FIG_MAIN M-file for fig_main.fig | ||
+ | % FIG_MAIN, by itself, creates a new FIG_MAIN or raises the existing | ||
+ | % singleton*. | ||
+ | % | ||
+ | % H = FIG_MAIN returns the handle to a new FIG_MAIN or the handle to | ||
+ | % the existing singleton*. | ||
+ | % | ||
+ | % FIG_MAIN('CALLBACK',hObject,eventData,handles,...) calls the local | ||
+ | % function named CALLBACK in FIG_MAIN.M with the given input arguments. | ||
+ | % | ||
+ | % FIG_MAIN('Property','Value',...) creates a new FIG_MAIN or raises the | ||
+ | % existing singleton*. Starting from the left, property value pairs are | ||
+ | % applied to the GUI before fig_main_OpeningFcn gets called. An | ||
+ | % unrecognized property name or invalid value makes property application | ||
+ | % stop. All inputs are passed to fig_main_OpeningFcn via varargin. | ||
+ | % | ||
+ | % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one | ||
+ | % instance to run (singleton)". | ||
+ | % | ||
+ | % See also: GUIDE, GUIDATA, GUIHANDLES | ||
+ | |||
+ | % Edit the above text to modify the response to help fig_main | ||
+ | |||
+ | % Last Modified by GUIDE v2.5 13-Jun-2012 12:18:01 | ||
+ | |||
+ | % Begin initialization code - DO NOT EDIT | ||
+ | gui_Singleton = 1; | ||
+ | gui_State = struct('gui_Name', mfilename, ... | ||
+ | 'gui_Singleton', gui_Singleton, ... | ||
+ | 'gui_OpeningFcn', @fig_main_OpeningFcn, ... | ||
+ | 'gui_OutputFcn', @fig_main_OutputFcn, ... | ||
+ | 'gui_LayoutFcn', [] , ... | ||
+ | 'gui_Callback', []); | ||
+ | if nargin && ischar(varargin{1}) | ||
+ | gui_State.gui_Callback = str2func(varargin{1}); | ||
+ | end | ||
+ | |||
+ | if nargout | ||
+ | [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); | ||
+ | else | ||
+ | gui_mainfcn(gui_State, varargin{:}); | ||
+ | end | ||
+ | % End initialization code - DO NOT EDIT | ||
+ | |||
+ | |||
+ | % --- Executes just before fig_main is made visible. | ||
+ | function fig_main_OpeningFcn(hObject, eventdata, handles, varargin) | ||
+ | % This function has no output args, see OutputFcn. | ||
+ | % hObject handle to figure | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | % varargin command line arguments to fig_main (see VARARGIN) | ||
+ | |||
+ | % Choose default command line output for fig_main | ||
+ | handles.output = hObject; | ||
+ | |||
+ | % Update handles structure | ||
+ | guidata(hObject, handles); | ||
+ | |||
+ | set(handles.fig_main, 'Units', 'pixels'); | ||
+ | set(0, 'Units', 'pixels'); | ||
+ | |||
+ | % Locate form | ||
+ | ScreenSize = get(0,'ScreenSize'); | ||
+ | if ((ScreenSize(3) < 1280)||(ScreenSize(4) < 720)) | ||
+ | msgbox('Sceeen size too small!'); | ||
+ | close(handles.fig_main); | ||
+ | end | ||
+ | FigWeigth = 1250; | ||
+ | FigWeigth_border = fix((ScreenSize(3) - FigWeigth)/2); | ||
+ | FigHeigth = 670; | ||
+ | FigHeigth_border = fix((ScreenSize(4) - FigHeigth)/2); | ||
+ | set(handles.fig_main, 'Position', [FigWeigth_border FigHeigth_border FigWeigth FigHeigth]); | ||
+ | set(handles.pan_Tracking, 'Position', get(handles.pan_Orbital_Elements, 'Position')); | ||
+ | set(handles.pan_Sch, 'Position', get(handles.pan_Orbital_Elements, 'Position')); | ||
+ | |||
+ | % Draw functional scheme | ||
+ | fig_main_pictureData = imread('Sch1.png'); | ||
+ | image(fig_main_pictureData, 'Parent', handles.axes_Sch1); | ||
+ | set(handles.axes_Sch1, 'XTick', []); | ||
+ | set(handles.axes_Sch1, 'YTick', []); | ||
+ | |||
+ | % Set widget's function | ||
+ | for i = 1:5 | ||
+ | for j = 1:5 | ||
+ | set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'DrawMode', 'fast'); | ||
+ | plot_axes_OE(handles, 0, [num2str(i) num2str(j)]); | ||
+ | set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'Tag', ['axes_OE_' num2str(i) num2str(j)]); | ||
+ | set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_OE_ButtonDownFcn'',hObject,eventdata,guidata(hObject))')); | ||
+ | pos = get(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'Position'); | ||
+ | pos(1) = pos(1) + (j-1)*45; | ||
+ | pos(2) = pos(2) - (i-1)*32 - 35; | ||
+ | pos(3) = pos(3) + 40; | ||
+ | pos(4) = pos(4) + 35; | ||
+ | set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'Position', pos); | ||
+ | end | ||
+ | end | ||
+ | for i = 1:5 | ||
+ | for j = 1:5 | ||
+ | set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'DrawMode', 'fast'); | ||
+ | plot_axes_Track(handles, 0, [num2str(i) num2str(j)]); | ||
+ | set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Tag', ['axes_Track_' num2str(i) num2str(j)]); | ||
+ | set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_Track_ButtonDownFcn'',hObject,eventdata,guidata(hObject))')); | ||
+ | pos = get(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Position'); | ||
+ | pos(1) = pos(1) + (j-1)*45; | ||
+ | pos(2) = pos(2) - (i-1)*32 - 35; | ||
+ | pos(3) = pos(3) + 40; | ||
+ | pos(4) = pos(4) + 35; | ||
+ | set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Position', pos); | ||
+ | end | ||
+ | end | ||
+ | plot_axes_Earth(handles, 0); | ||
+ | globals; | ||
+ | |||
+ | % Set constants | ||
+ | start_handle = handles; | ||
+ | |||
+ | |||
+ | % UIWAIT makes fig_main wait for user response (see UIRESUME) | ||
+ | % uiwait(handles.fig_main); | ||
+ | |||
+ | |||
+ | % --- Outputs from this function are returned to the command line. | ||
+ | function varargout = fig_main_OutputFcn(hObject, eventdata, handles) | ||
+ | % varargout cell array for returning output args (see VARARGOUT); | ||
+ | % hObject handle to figure | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | |||
+ | % Get default command line output from handles structure | ||
+ | varargout{1} = handles.output; | ||
+ | |||
+ | |||
+ | % --- Executes on mouse press over axes background. | ||
+ | function axes_3D_ButtonDownFcn(hObject, eventdata, handles) | ||
+ | % hObject handle to axes_3D (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | globals; | ||
+ | plot_axes_Earth(handles, next_hF()); | ||
+ | |||
+ | |||
+ | % --- Executes on mouse press over axes background. | ||
+ | function axes_OE_ButtonDownFcn(hObject, eventdata, handles) | ||
+ | % hObject handle to axes_OE_ (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | dig = get(hObject, 'Tag'); dig = dig(end-1:end); | ||
+ | plot_axes_OE(handles, next_hF(), dig) | ||
+ | |||
+ | |||
+ | % --- Executes on mouse press over axes background. | ||
+ | function axes_Track_ButtonDownFcn(hObject, eventdata, handles) | ||
+ | % hObject handle to axes_OE_ (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | dig = get(hObject, 'Tag'); dig = dig(end-1:end); | ||
+ | plot_axes_Track(handles, next_hF(), dig) | ||
+ | |||
+ | |||
+ | % --- Executes on button press in pb_Track. | ||
+ | function pb_Track_Callback(hObject, eventdata, handles) | ||
+ | % hObject handle to pb_Track (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | std_x = str2double(get(handles.ed_stdX, 'String')); | ||
+ | if ~((std_x >= 0)&&(std_x < 1000)) | ||
+ | msgbox('Coordinate''s RMS is incorrect', 'Error', 'error'); | ||
+ | set(handles.ed_stdX, 'String', '10'); | ||
+ | return | ||
+ | end | ||
+ | std_V = str2double(get(handles.ed_stdV, 'String')); | ||
+ | if ~((std_V >= 0)&&(std_V < 5)) | ||
+ | msgbox('Velocity''s RMS is incorrect', 'Error', 'error'); | ||
+ | set(handles.ed_stdV, 'String', '0.01'); | ||
+ | return | ||
+ | end | ||
+ | track_with_noise(handles, std_x, std_V); | ||
+ | |||
+ | % --- Executes on button press in pb_True. | ||
+ | function pb_True_Callback(hObject, eventdata, handles) | ||
+ | % hObject handle to pb_True (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | set(handles.pan_Orbital_Elements, 'Visible', 'on'); | ||
+ | set(handles.pan_Tracking, 'Visible', 'off'); | ||
+ | set(handles.pan_Sch, 'Visible', 'off'); | ||
+ | |||
+ | |||
+ | % --- Executes on button press in pb_Tracking. | ||
+ | function pb_Tracking_Callback(hObject, eventdata, handles) | ||
+ | % hObject handle to pb_Tracking (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | set(handles.pan_Orbital_Elements, 'Visible', 'off'); | ||
+ | set(handles.pan_Tracking, 'Visible', 'on'); | ||
+ | set(handles.pan_Sch, 'Visible', 'off'); | ||
+ | |||
+ | |||
+ | % --- Executes on button press in pb_Solve. | ||
+ | function pb_Solve_Callback(hObject, eventdata, handles) | ||
+ | % hObject handle to pb_Solve (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | solve_wo_noise(handles); | ||
+ | |||
+ | |||
+ | |||
+ | function ed_stdX_Callback(hObject, eventdata, handles) | ||
+ | % hObject handle to ed_stdX (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | |||
+ | % Hints: get(hObject,'String') returns contents of ed_stdX as text | ||
+ | % str2double(get(hObject,'String')) returns contents of ed_stdX as a double | ||
+ | |||
+ | |||
+ | % --- Executes during object creation, after setting all properties. | ||
+ | function ed_stdX_CreateFcn(hObject, eventdata, handles) | ||
+ | % hObject handle to ed_stdX (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles empty - handles not created until after all CreateFcns called | ||
+ | |||
+ | % Hint: edit controls usually have a white background on Windows. | ||
+ | % See ISPC and COMPUTER. | ||
+ | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) | ||
+ | set(hObject,'BackgroundColor','white'); | ||
+ | end | ||
+ | |||
+ | |||
+ | |||
+ | function ed_stdV_Callback(hObject, eventdata, handles) | ||
+ | % hObject handle to ed_stdV (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | |||
+ | % Hints: get(hObject,'String') returns contents of ed_stdV as text | ||
+ | % str2double(get(hObject,'String')) returns contents of ed_stdV as a double | ||
+ | |||
+ | |||
+ | % --- Executes during object creation, after setting all properties. | ||
+ | function ed_stdV_CreateFcn(hObject, eventdata, handles) | ||
+ | % hObject handle to ed_stdV (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles empty - handles not created until after all CreateFcns called | ||
+ | |||
+ | % Hint: edit controls usually have a white background on Windows. | ||
+ | % See ISPC and COMPUTER. | ||
+ | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) | ||
+ | set(hObject,'BackgroundColor','white'); | ||
+ | end | ||
+ | |||
+ | |||
+ | % --- Executes on button press in pb_Sch. | ||
+ | function pb_Sch_Callback(hObject, eventdata, handles) | ||
+ | % hObject handle to pb_Sch (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | set(handles.pan_Orbital_Elements, 'Visible', 'off'); | ||
+ | set(handles.pan_Tracking, 'Visible', 'off'); | ||
+ | set(handles.pan_Sch, 'Visible', 'on'); | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===drawErrors.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | %> ====================================================================== | ||
+ | %> @brief plot Caclulate and draw RMSes | ||
+ | %> @param handles Main handles struct | ||
+ | % ====================================================================== | ||
+ | function draw_Errors(handles) | ||
+ | globals; | ||
+ | begin_i = Nmod - 300; end_i = Nmod; | ||
+ | |||
+ | set(handles.txt_Err_11, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.e(begin_i:end_i) - Xist.e(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_12, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_e(begin_i:end_i) - Xist.d_e(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_21, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.p(begin_i:end_i) - Xist.p(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_22, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_p(begin_i:end_i) - Xist.d_p(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_31, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.theta(begin_i:end_i) - Xist.theta(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_32, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_theta(begin_i:end_i) - Xist.d_theta(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_41, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.omega(begin_i:end_i) - Xist.omega(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_42, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_omega(begin_i:end_i) - Xist.d_omega(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_51, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.Omega(begin_i:end_i) - Xist.Omega(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_52, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_Omega(begin_i:end_i) - Xist.d_Omega(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_61, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.i(begin_i:end_i) - Xist.i(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_62, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_i(begin_i:end_i) - Xist.d_i(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_71, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.x0(begin_i:end_i) - Xist.x0(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_72, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_x0(begin_i:end_i) - Xist.d_x0(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_81, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.y0(begin_i:end_i) - Xist.y0(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_82, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_y0(begin_i:end_i) - Xist.d_y0(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_91, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.z0(begin_i:end_i) - Xist.z0(begin_i:end_i)).^2))))); | ||
+ | set(handles.txt_Err_92, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_z0(begin_i:end_i) - Xist.d_z0(begin_i:end_i)).^2))))); | ||
+ | |||
+ | end | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===next_hF.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | function hF_cont = next_hF() | ||
+ | global hF_cont | ||
+ | |||
+ | hF_cont = hF_cont + 1; | ||
+ | end | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===plot_axes_Earth.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | %> ====================================================================== | ||
+ | %> @brief 3D-Earth, SV's orbits | ||
+ | %> @param handles Main handles struct | ||
+ | %> @param hF Flag (handle) for separate window | ||
+ | % ====================================================================== | ||
+ | function plot_axes_Earth(handles, hF) | ||
+ | globals; | ||
+ | |||
+ | R_earth_pol = 6356777; % Polar radius of Earth | ||
+ | R_earth_equa = 6378160; % Equatorial radius of Earth | ||
+ | Earth_axe_angl = deg2rad(23); | ||
+ | |||
+ | [x,y,z] = sphere(50); | ||
+ | x = R_earth_equa * x; y = R_earth_equa * y; z = R_earth_pol * z; | ||
+ | load('topo.mat','topo','topomap1'); | ||
+ | % cla reset | ||
+ | % axis square off | ||
+ | props.AmbientStrength = 0.1; | ||
+ | props.DiffuseStrength = 1; | ||
+ | props.SpecularColorReflectance = .5; | ||
+ | props.SpecularExponent = 20; | ||
+ | props.SpecularStrength = 1; | ||
+ | props.FaceColor= 'texture'; | ||
+ | props.EdgeColor = 'none'; | ||
+ | props.FaceLighting = 'phong'; | ||
+ | props.Cdata = topo; | ||
+ | alpha_earth = pi/1; | ||
+ | x_new = x*cos(alpha_earth) + y*sin(alpha_earth); | ||
+ | y_new = -x*sin(alpha_earth) + y*cos(alpha_earth); | ||
+ | |||
+ | if hF | ||
+ | figure(hF); | ||
+ | hA = gca; | ||
+ | else | ||
+ | set(handles.fig_main,'CurrentAxes', handles.axes_3D) | ||
+ | hA = handles.axes_3D; | ||
+ | end | ||
+ | |||
+ | cla(hA); | ||
+ | surface(x_new, y_new, z, props); | ||
+ | light('position',[-1 -1 1*tan(Earth_axe_angl)]*R_earth_equa*2); | ||
+ | % campos([+1 0 -1*tan(Earth_axe_angl)]*R_earth_equa*2); | ||
+ | % camtarget([0 0 0]) | ||
+ | view(3) | ||
+ | view(40,-8); | ||
+ | hold(hA, 'on'); | ||
+ | plot3(hA, Xest.x, Xest.y, Xest.z, Xist.x, Xist.y, Xist.z, 'LineWidth', 2); | ||
+ | hold(hA, 'off'); | ||
+ | |||
+ | xlim([-8e6 8e6]); ylim([-8e6 8e6]); zlim([-8e6 8e6]); | ||
+ | title(hA, 'Space View'); | ||
+ | if ~hF | ||
+ | set(hA, 'XTick', []); | ||
+ | set(hA, 'YTick', []); | ||
+ | set(hA, 'ZTick', []); | ||
+ | set(hA, 'FontSize', Font_Size); | ||
+ | else | ||
+ | xlabel(hA, 'x, m'); | ||
+ | ylabel(hA, 'y, m'); | ||
+ | zlabel(hA, 'z, m'); | ||
+ | grid(hA, 'on'); | ||
+ | end | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===plot_axes_OE.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | %> ====================================================================== | ||
+ | %> @brief plot True trajectory graphs | ||
+ | %> @param handles Main handles struct | ||
+ | %> @param hF Flag (handle) for separate window | ||
+ | %> @param ij Indexies of axes (2-el string) | ||
+ | % ====================================================================== | ||
+ | function plot_axes_OE(handles, hF, ij) | ||
+ | globals; | ||
+ | |||
+ | if hF | ||
+ | figure(hF); | ||
+ | hA = gca; | ||
+ | XLab = 't, s'; | ||
+ | else | ||
+ | hA = eval(['handles.axes_OE_' ij]); | ||
+ | set(handles.fig_main,'CurrentAxes', hA) | ||
+ | XLab = ''; | ||
+ | YLab = ''; | ||
+ | end | ||
+ | titl = ''; | ||
+ | X1 = tmod; X2 = tmod; X3 = tmod; X4 = tmod; | ||
+ | Y1 = nan(1, Nmod); Y2 = nan(1, Nmod); Y3 = nan(1, Nmod); Y4 = nan(1, Nmod); | ||
+ | leg_str =''; | ||
+ | switch ij | ||
+ | case '11' | ||
+ | Y1 = Xist.sqrtA; | ||
+ | YLab = 'a^{1/2}, m^{1/2}'; | ||
+ | titl = 'Square root of semimajor axis'; | ||
+ | case '12' | ||
+ | Y1 = Xist.e; | ||
+ | YLab = 'e'; | ||
+ | titl = 'Eccentricity'; | ||
+ | case '13' | ||
+ | Y1 = Xist.Crc; | ||
+ | Y2 = Xist.Crs; | ||
+ | YLab = 'C_{rc}, C_{rs}, m'; | ||
+ | titl = 'Harmonic coefficients for radius'; | ||
+ | leg_str = 'legend(hA, ''C_{rc}'', ''C_{rs}'')'; | ||
+ | case '14' | ||
+ | Y1 = Xist.r; | ||
+ | Y2 = Xist.A; | ||
+ | YLab = 'r, a, m'; | ||
+ | titl = 'Full radius, semimajor axis'; | ||
+ | leg_str = 'legend(hA, ''Radius'', ''Semimajor axis'')'; | ||
+ | case '21' | ||
+ | Y1 = Xist.i0; | ||
+ | YLab = 'i_0, rad'; | ||
+ | titl = 'Base inclination'; | ||
+ | case '22' | ||
+ | Y1 = Xist.i_dot; | ||
+ | YLab = 'i_{dot}, rad/s'; | ||
+ | titl = 'Derivative of the base inclination'; | ||
+ | case '23' | ||
+ | Y1 = Xist.Cic; | ||
+ | Y2 = Xist.Cis; | ||
+ | YLab = 'C_{ic}, C_{is}, rad'; | ||
+ | titl = 'Harmonic coefficients for inclination'; | ||
+ | leg_str = 'legend(hA, ''C_{ic}'', ''C_{is}'')'; | ||
+ | case '24' | ||
+ | Y1 = Xist.i; | ||
+ | YLab = 'i, rad'; | ||
+ | titl = 'Full inclination'; | ||
+ | case '31' | ||
+ | Y1 = Xist.M0; | ||
+ | Y2 = Xist.E; | ||
+ | Y3 = Xist.theta; | ||
+ | YLab = 'M_0, E, \theta, rad'; | ||
+ | titl = 'Mean, eccentricity and true anomaly'; | ||
+ | leg_str = 'legend(hA, ''M_0'', ''E'', ''\theta'')'; | ||
+ | case '32' | ||
+ | Y1 = Xist.omega; | ||
+ | YLab = '\omega, rad'; | ||
+ | titl = 'Argument of periapsis'; | ||
+ | case '33' | ||
+ | Y1 = Xist.Cuc; | ||
+ | Y2 = Xist.Cus; | ||
+ | YLab = 'C_{uc}, C_{us}, rad'; | ||
+ | titl = 'Harmonic coefficients for argument of latitude'; | ||
+ | leg_str = 'legend(hA, ''C_{uc}'', ''C_{us}'')'; | ||
+ | case '34' | ||
+ | Y1 = Xist.u; | ||
+ | YLab = 'u, rad'; | ||
+ | titl = 'Argument of latitude'; | ||
+ | case '41' | ||
+ | Y1 = Xist.Omega; | ||
+ | YLab = '\Omega, rad'; | ||
+ | titl = 'Longitude of the ascending node'; | ||
+ | case '42' | ||
+ | Y1 = Xist.Omega_dot; | ||
+ | YLab = '\Omega_{dot}, rad/s'; | ||
+ | titl = 'Derivative of the longitude of the ascending node'; | ||
+ | case '43' | ||
+ | Y1 = Xist.x0; | ||
+ | Y2 = Xist.y0; | ||
+ | Y3 = Xist.z0; | ||
+ | YLab = 'x_0, y_0, z_0, m'; | ||
+ | titl = 'Coordinates in the inertial coordinate system'; | ||
+ | leg_str = 'legend(hA, ''x_0'', ''y_0'', ''z_0'')'; | ||
+ | case '44' | ||
+ | Y1 = Xist.d_x0; | ||
+ | Y2 = Xist.d_y0; | ||
+ | Y3 = Xist.d_z0; | ||
+ | YLab = 'V_x, V_y, V_z, m'; | ||
+ | titl = 'Velocities in the inertial coordinate system'; | ||
+ | leg_str = 'legend(hA, ''V_x'', ''V_y'', ''V_z'')'; | ||
+ | end | ||
+ | |||
+ | if isnan(Y4(1)) | ||
+ | if isnan(Y3(1)) | ||
+ | if isnan(Y2(1)) | ||
+ | if isnan(Y1(1)) | ||
+ | set(hA, 'Visible', 'off'); | ||
+ | else | ||
+ | plot(hA, X1, Y1); | ||
+ | set(hA, 'Visible', 'on'); | ||
+ | end | ||
+ | else | ||
+ | plot(hA, X1, Y1, X2, Y2); | ||
+ | set(hA, 'Visible', 'on'); | ||
+ | end | ||
+ | else | ||
+ | plot(hA, X1, Y1, X2, Y2, X3, Y3); | ||
+ | set(hA, 'Visible', 'on'); | ||
+ | end | ||
+ | else | ||
+ | plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4); | ||
+ | set(hA, 'Visible', 'on'); | ||
+ | end | ||
+ | |||
+ | if hF | ||
+ | title(hA, titl); | ||
+ | eval(leg_str); | ||
+ | end | ||
+ | |||
+ | ly = ylabel(YLab); | ||
+ | lx = xlabel(XLab); | ||
+ | grid(hA, 'on'); | ||
+ | |||
+ | if ~hF | ||
+ | set(hA, 'XTick', []); | ||
+ | set(hA, 'YTick', []); | ||
+ | set(ly, 'FontSize', Font_Size); | ||
+ | end | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===plot_axes_Track.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | %> ====================================================================== | ||
+ | %> @brief plot Tracking graphs | ||
+ | %> @param handles Main handles struct | ||
+ | %> @param hF Flag (handle) for separate window | ||
+ | %> @param ij Indexies of axes (2-el string) | ||
+ | % ====================================================================== | ||
+ | function plot_axes_Track(handles, hF, ij) | ||
+ | globals; | ||
+ | |||
+ | if hF | ||
+ | figure(hF); | ||
+ | hA = gca; | ||
+ | XLab = 't, s'; | ||
+ | else | ||
+ | hA = eval(['handles.axes_Track_' ij]); | ||
+ | set(handles.fig_main,'CurrentAxes', hA) | ||
+ | XLab = ''; | ||
+ | YLab = ''; | ||
+ | end | ||
+ | |||
+ | titl = ''; | ||
+ | X1 = tmod; X2 = tmod; X3 = tmod; X4 = tmod; X5 = tmod; X6 = tmod; | ||
+ | Y1 = nan(1, Nmod); Y2 = nan(1, Nmod); Y3 = nan(1, Nmod); Y4 = nan(1, Nmod); Y5 = nan(1, Nmod); Y6 = nan(1, Nmod); | ||
+ | switch ij | ||
+ | case '11' | ||
+ | Y1 = Xest.e; | ||
+ | Y2 = Xest_won.e; | ||
+ | YLab = 'e'; | ||
+ | titl = 'Eccentricity'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '12' | ||
+ | Y1 = Xest.d_e; | ||
+ | Y2 = Xest_won.d_e; | ||
+ | YLab = 'e'''; | ||
+ | titl = 'Eccentricity rate'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '13' | ||
+ | Y1 = Xest.p; | ||
+ | Y2 = Xest_won.p; | ||
+ | YLab = 'p, m'; | ||
+ | titl = 'Focal parameter'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '14' | ||
+ | Y1 = Xest.d_p; | ||
+ | Y2 = Xest_won.d_p; | ||
+ | YLab = 'p'', m/s'; | ||
+ | titl = 'Focal parameter rate'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '21' | ||
+ | Y1 = Xest.theta; | ||
+ | Y2 = Xest_won.theta; | ||
+ | YLab = '\theta, rad'; | ||
+ | titl = 'True anomaly'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '22' | ||
+ | Y1 = Xest.d_theta; | ||
+ | Y2 = Xest_won.d_theta; | ||
+ | YLab = '\theta'', rad/s'; | ||
+ | titl = 'True anomaly rate'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '23' | ||
+ | Y1 = Xest.omega; | ||
+ | Y2 = Xest_won.omega; | ||
+ | YLab = '\omega, rad'; | ||
+ | titl = 'Argument of periapsis'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '24' | ||
+ | Y1 = Xest.d_omega; | ||
+ | Y2 = Xest_won.d_omega; | ||
+ | YLab = '\omega'', rad/s'; | ||
+ | titl = 'Argument of periapsis rate'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '31' | ||
+ | Y1 = Xest.Omega; | ||
+ | Y2 = Xest_won.Omega; | ||
+ | YLab = '\Omega, rad'; | ||
+ | titl = 'Longitude of the ascending node'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '32' | ||
+ | Y1 = Xest.d_Omega; | ||
+ | Y2 = Xest_won.d_Omega; | ||
+ | YLab = '\Omega'', rad/s'; | ||
+ | titl = 'Longitude of the ascending node rate'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '33' | ||
+ | Y1 = Xest.i; | ||
+ | Y2 = Xest_won.i; | ||
+ | YLab = 'i, rad'; | ||
+ | titl = 'Inclination'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '34' | ||
+ | Y1 = Xest.d_i; | ||
+ | Y2 = Xest_won.d_i; | ||
+ | YLab = 'i'', rad/s'; | ||
+ | titl = 'Inclination rate'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '41' | ||
+ | Y1 = Xest.x0; | ||
+ | Y2 = Xest.y0; | ||
+ | Y3 = Xest.z0; | ||
+ | Y4 = Xest_won.x0; | ||
+ | Y5 = Xest_won.y0; | ||
+ | Y6 = Xest_won.z0; | ||
+ | YLab = 'x_0, y_0, z_0, m'; | ||
+ | titl = 'Coordinates in the inertial coordinate system'; | ||
+ | leg_str = 'legend(hA, ''Estimation x_0'', ''Estimation y_0'', ''Estimation z_0'', ''True x_0'', ''True y_0'', ''True z_0'')'; | ||
+ | case '42' | ||
+ | Y1 = Xest.d_x0; | ||
+ | Y2 = Xest.d_y0; | ||
+ | Y3 = Xest.d_z0; | ||
+ | Y4 = Xest_won.d_x0; | ||
+ | Y5 = Xest_won.d_y0; | ||
+ | Y6 = Xest_won.d_z0; | ||
+ | YLab = 'V_x, V_y, V_z, m/s'; | ||
+ | titl = 'Velocities in the inertial coordinate system'; | ||
+ | leg_str = 'legend(hA, ''Estimation V_x'', ''Estimation V_y'', ''Estimation V_z'', ''True V_x'', ''True V_y'', ''True V_z'')'; | ||
+ | case '43' | ||
+ | Y1 = sqrt((Xest.x0 - Xist.x0).^2 + (Xest.y0 - Xist.y0).^2 + (Xest.z0 - Xist.z0).^2); | ||
+ | Y2 = sqrt((Xest_won.x0 - Xist.x0).^2 + (Xest_won.y0 - Xist.y0).^2 + (Xest_won.z0 - Xist.z0).^2); | ||
+ | YLab = 'Error XYZ, m'; | ||
+ | titl = 'Error of coordinate estimation'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | case '44' | ||
+ | Y1 = sqrt((Xest.d_x0 - Xist.d_x0).^2 + (Xest.d_y0 - Xist.d_y0).^2 + (Xest.d_z0 - Xist.d_z0).^2); | ||
+ | Y2 = sqrt((Xest_won.d_x0 - Xist.d_x0).^2 + (Xest_won.d_y0 - Xist.d_y0).^2 + (Xest_won.d_z0 - Xist.d_z0).^2); | ||
+ | YLab = 'Error V, m/s'; | ||
+ | titl = 'Error of velocity estimation'; | ||
+ | leg_str = 'legend(hA, ''Estimation'', ''True'')'; | ||
+ | end | ||
+ | |||
+ | if isnan(Y6(1)) | ||
+ | if isnan(Y5(1)) | ||
+ | if isnan(Y4(1)) | ||
+ | if isnan(Y3(1)) | ||
+ | if isnan(Y2(1)) | ||
+ | if isnan(Y1(1)) | ||
+ | set(hA, 'Visible', 'off'); | ||
+ | else | ||
+ | plot(hA, X1, Y1); | ||
+ | set(hA, 'Visible', 'on'); | ||
+ | end | ||
+ | else | ||
+ | plot(hA, X1, Y1, X2, Y2); | ||
+ | set(hA, 'Visible', 'on'); | ||
+ | end | ||
+ | else | ||
+ | plot(hA, X1, Y1, X2, Y2, X3, Y3); | ||
+ | set(hA, 'Visible', 'on'); | ||
+ | end | ||
+ | else | ||
+ | plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4); | ||
+ | set(hA, 'Visible', 'on'); | ||
+ | end | ||
+ | else | ||
+ | plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4, X5, Y5); | ||
+ | set(hA, 'Visible', 'on'); | ||
+ | end | ||
+ | else | ||
+ | plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4, X5, Y5, X6, Y6); | ||
+ | set(hA, 'Visible', 'on'); | ||
+ | end | ||
+ | |||
+ | |||
+ | if hF | ||
+ | title(hA, titl); | ||
+ | eval(leg_str); | ||
+ | end | ||
+ | |||
+ | ly = ylabel(YLab); | ||
+ | lx = xlabel(XLab); | ||
+ | grid(hA, 'on'); | ||
+ | |||
+ | if ~hF | ||
+ | set(hA, 'XTick', []); | ||
+ | set(hA, 'YTick', []); | ||
+ | set(ly, 'FontSize', Font_Size); | ||
+ | end | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===fsolve_Kepler.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | function Ysolve = fsolve_Kepler(Xs, Xizm, Yizm, Zizm, VXizm, VYizm, VZizm) | ||
+ | |||
+ | global mu_earth | ||
+ | |||
+ | %%%%Xsolve = r, u, Omega, i, d_r, d_u | ||
+ | % Xsolve = theta, omega_p, Omega, i, e, p | ||
+ | theta = Xs(1); | ||
+ | omega_p = Xs(2); | ||
+ | Omega = Xs(3); | ||
+ | i = Xs(4); | ||
+ | e = Xs(5); | ||
+ | p = Xs(6); | ||
+ | |||
+ | [x, y, z, Vx, Vy, Vz] = get_vector_XV( e, p, theta, omega_p, Omega, i); | ||
+ | |||
+ | ErrX = Xizm - x; | ||
+ | ErrY = Yizm - y; | ||
+ | ErrZ = Zizm - z; | ||
+ | |||
+ | ErrVx = VXizm - Vx; | ||
+ | ErrVy = VYizm - Vy; | ||
+ | ErrVz = VZizm - Vz; | ||
+ | |||
+ | Ysolve = [ErrX, ErrY, ErrZ, ErrVx, ErrVy, ErrVz]; | ||
+ | |||
+ | end | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===solve_wo_noise.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | %> ====================================================================== | ||
+ | %> @brief Calculate true osculating elements for true trajectory | ||
+ | %> @param handles Main handles struct | ||
+ | %> ====================================================================== | ||
+ | function solve_wo_noise( handles ) | ||
+ | |||
+ | globals; | ||
+ | |||
+ | % Initial point | ||
+ | Xs(1) = Xist.theta(1); | ||
+ | Xs(2) = Xist.omega(1); | ||
+ | Xs(3) = Xist.Omega(1); | ||
+ | Xs(4) = Xist.i(1); | ||
+ | Xs(5) = Xist.e(1); | ||
+ | Xs(6) = Xist.p(1); | ||
+ | |||
+ | set(handles.pb_Solve, 'Enable', 'off'); | ||
+ | drawnow | ||
+ | pause(0.01); | ||
+ | for i = 1:Nmod | ||
+ | |||
+ | % Calculating osculating elements by means of the true coordinates and velocities | ||
+ | Xs = fsolve(@(Xfs)(fsolve_Kepler(Xfs, Xist.x0(i), Xist.y0(i), Xist.z0(i),... | ||
+ | Xist.d_x0(i), Xist.d_y0(i), Xist.d_z0(i))), Xs, options_solve); | ||
+ | |||
+ | % Saving of results | ||
+ | Xest_won.e(i) = Xs(5); | ||
+ | Xest_won.theta(i) = Xs(1); | ||
+ | Xest_won.omega(i) = Xs(2); | ||
+ | Xest_won.Omega(i) = Xs(3); | ||
+ | Xest_won.i(i) = Xs(4); | ||
+ | Xest_won.p(i) = Xs(6); | ||
+ | Xest_won.r(i) = Xest_won.p(i) ./ (1 + Xest_won.e(i)*cos(Xest_won.theta(i))); | ||
+ | |||
+ | % Get coordinates for oculating elements (for test) | ||
+ | [Xest_won.x0(i) Xest_won.y0(i) Xest_won.z0(i) ... | ||
+ | Xest_won.d_x0(i) Xest_won.d_y0(i) Xest_won.d_z0(i)] = ... | ||
+ | get_vector_XV( Xest_won.e(i), Xest_won.p(i), Xest_won.theta(i), ... | ||
+ | Xest_won.omega(i), Xest_won.Omega(i), Xest_won.i(i)); | ||
+ | % [Xest_won.x0(i) Xest_won.y0(i) Xest_won.z0(i)] = get_vector_XYZ( Xest_won.r(i),... | ||
+ | % Xest_won.Omega(i), Xest_won.i(i), Xest_won.theta(i)+Xest_won.omega(i) ); | ||
+ | |||
+ | if ~mod(i, fix(Nmod/100)) | ||
+ | set(handles.txt_Solve, 'String', sprintf('%.0f %%', i/Nmod*100)); | ||
+ | end | ||
+ | drawnow | ||
+ | pause(0.01); | ||
+ | end | ||
+ | set(handles.txt_Solve, 'String', sprintf('%.0f %%', 100)); | ||
+ | |||
+ | % Calculation of derivatives | ||
+ | Xest_won.d_theta = diff(Xest_won.theta)/dTmod; | ||
+ | Xest_won.d_theta(end+1)=Xest_won.d_theta(end); | ||
+ | Xest_won.d_omega = diff(Xest_won.omega)/dTmod; | ||
+ | Xest_won.d_omega(end+1)=Xest_won.d_omega(end); | ||
+ | Xest_won.d_Omega = diff(Xest_won.Omega)/dTmod; | ||
+ | Xest_won.d_Omega(end+1)=Xest_won.d_Omega(end); | ||
+ | Xest_won.d_i = diff(Xest_won.i)/dTmod; | ||
+ | Xest_won.d_i(end+1)=Xest_won.d_i(end); | ||
+ | Xest_won.d_e = diff(Xest_won.e)/dTmod; | ||
+ | Xest_won.d_e(end+1)=Xest_won.d_e(end); | ||
+ | Xest_won.d_p = diff(Xest_won.p)/dTmod; | ||
+ | Xest_won.d_p(end+1)=Xest_won.d_p(end); | ||
+ | Xest_won.d_r = diff(Xest_won.r)/dTmod; | ||
+ | Xest_won.d_r(end+1)=Xest_won.d_r(end); | ||
+ | |||
+ | for i = 1:Nmod | ||
+ | % Saving of results | ||
+ | if Xest_won.e(i) > 0; | ||
+ | Xest_won.e(i) = Xest_won.e(i); | ||
+ | Xest_won.theta(i) = mod_pm_pi(Xest_won.theta(i)); | ||
+ | Xest_won.omega(i) = mod_pm_pi(Xest_won.omega(i)); | ||
+ | else | ||
+ | Xest_won.e(i) = -Xest_won.e(i); | ||
+ | Xest_won.theta(i) = mod_pm_pi(-Xest_won.theta(i)); | ||
+ | Xest_won.omega(i) = mod_pm_pi(-Xest_won.omega(i)); | ||
+ | end | ||
+ | Xest_won.Omega(i) = mod_pm_pi(Xest_won.Omega(i)); | ||
+ | Xest_won.i(i) = mod_pm_pi(Xest_won.i(i)); | ||
+ | end | ||
+ | |||
+ | % Output results to form | ||
+ | for i = 1:5 | ||
+ | for j = 1:5 | ||
+ | set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'DrawMode', 'fast'); | ||
+ | plot_axes_Track(handles, 0, [num2str(i) num2str(j)]); | ||
+ | set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Tag', ['axes_Track_' num2str(i) num2str(j)]); | ||
+ | set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_Track_ButtonDownFcn'',hObject,eventdata,guidata(hObject))')); | ||
+ | end | ||
+ | end | ||
+ | |||
+ | set(handles.pan_Orbital_Elements, 'Visible', 'off'); | ||
+ | set(handles.pan_Tracking, 'Visible', 'on'); | ||
+ | set(handles.pb_Track, 'Enable', 'on'); | ||
+ | set(handles.ed_stdX, 'Enable', 'on'); | ||
+ | set(handles.ed_stdV, 'Enable', 'on'); | ||
+ | |||
+ | end | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===get_vector_XV.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | function [x, y, z, Vx, Vy, Vz] = get_vector_XV( e, p, theta, omega, Omega, i) | ||
+ | %GET_VECTOR_XV Get vectors x, y, z and Vx, Vy, Vz | ||
+ | |||
+ | global mu_earth | ||
+ | |||
+ | munapi = sqrt(mu_earth / p); | ||
+ | Vr = munapi*e*sin(theta); | ||
+ | Vu = munapi*(1+e*cos(theta)); | ||
+ | r = p / (1+e*cos(theta)); | ||
+ | u = theta + omega; | ||
+ | |||
+ | xyz = U3(-Omega)*U1(-i)*U3(-u)*[r; 0; 0]; | ||
+ | |||
+ | x = xyz(1); | ||
+ | y = xyz(2); | ||
+ | z = xyz(3); | ||
+ | |||
+ | Vx = Vr.*x./r - Vu.*(sin(u).*cos(Omega) + cos(u).*sin(Omega).*cos(i)); | ||
+ | Vy = Vr.*y./r - Vu.*(sin(u).*sin(Omega) - cos(u).*cos(Omega).*cos(i)); | ||
+ | Vz = Vr.*z./r + Vu.*cos(u).*sin(i); | ||
+ | |||
+ | end | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===mod_pm_pi.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | function y = mod_pm_pi( x ) | ||
+ | %MOD_PM_PI mod [-pi; pi]; | ||
+ | |||
+ | y = mod(x + pi, 2*pi) - pi; | ||
+ | |||
+ | end | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===resize_arrays.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | |||
+ | tmod = tmod(1:Nmod); | ||
+ | |||
+ | %tput; | ||
+ | |||
+ | |||
+ | % --- Executes on mouse press over axes background. | ||
+ | function axes_3D_ButtonDownFcn(hObject, eventdata, handles) | ||
+ | % hObject handle to axes_3D (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | globals; | ||
+ | plot_axes_Earth(handles, next_hF()); | ||
+ | |||
+ | |||
+ | % --- Executes on mouse press over axes background. | ||
+ | function axes_OE_ButtonDownFcn(hObject, eventdata, handles) | ||
+ | % hObject handle to axes_OE_ (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | dig = get(hObject, 'Tag'); dig = dig(end-1:end); | ||
+ | plot_axes_OE(handles, next_hF(), dig) | ||
+ | |||
+ | |||
+ | % --- Executes on mouse press over axes background. | ||
+ | function axes_Track_ButtonDownFcn(hObject, eventdata, handles) | ||
+ | % hObject handle to axes_OE_ (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | dig = get(hObject, 'Tag'); dig = dig(end-1:end); | ||
+ | plot_axes_Track(handles, next_hF(), dig) | ||
+ | |||
+ | |||
+ | % --- Executes on button press in pb_Track. | ||
+ | function pb_Track_Callback(hObject, eventdata, handles) | ||
+ | % hObject handle to pb_Track (see GCBO) | ||
+ | % eventdata reserved - to be defined in a future version of MATLAB | ||
+ | % handles structure with handles and user data (see GUIDATA) | ||
+ | std_x = str2double(get(handles.ed_stdX, 'String')); | ||
+ | if ~((std_x >= 0) True vector | ||
+ | Xist.dn = Xist.dn(1:Nmod); | ||
+ | Xist.M0 = Xist.M0(1:Nmod); | ||
+ | Xist.e = Xist.e(1:Nmod); | ||
+ | Xist.d_e = Xist.d_e(1:Nmod); | ||
+ | Xist.dd_e = Xist.dd_e(1:Nmod); | ||
+ | Xist.A = Xist.A(1:Nmod); | ||
+ | Xist.d_A = Xist.d_A(1:Nmod); | ||
+ | Xist.dd_A = Xist.dd_A(1:Nmod); | ||
+ | Xist.sqrtA = Xist.sqrtA(1:Nmod); | ||
+ | Xist.Omega = Xist.Omega(1:Nmod); | ||
+ | Xist.d_Omega = Xist.d_Omega(1:Nmod); | ||
+ | Xist.dd_Omega = Xist.dd_Omega(1:Nmod); | ||
+ | Xist.lambda = Xist.lambda(1:Nmod); | ||
+ | Xist.d_lambda = Xist.d_lambda(1:Nmod); | ||
+ | Xist.dd_lambda = Xist.dd_lambda(1:Nmod); | ||
+ | Xist.Omega_dot = Xist.Omega_dot(1:Nmod); | ||
+ | Xist.Cis = Xist.Cis(1:Nmod); | ||
+ | Xist.Cic = Xist.Cic(1:Nmod); | ||
+ | Xist.Crs = Xist.Crs(1:Nmod); | ||
+ | Xist.Crc = Xist.Crc(1:Nmod); | ||
+ | Xist.Cus = Xist.Cus(1:Nmod); | ||
+ | Xist.Cuc = Xist.Cuc(1:Nmod); | ||
+ | Xist.omega = Xist.omega(1:Nmod); | ||
+ | Xist.d_omega = Xist.d_omega(1:Nmod); | ||
+ | Xist.dd_omega = Xist.dd_omega(1:Nmod); | ||
+ | Xist.i0 = Xist.i0(1:Nmod); | ||
+ | Xist.i_dot = Xist.i_dot(1:Nmod); | ||
+ | Xist.i = Xist.i(1:Nmod); | ||
+ | Xist.d_i = Xist.d_i(1:Nmod); | ||
+ | Xist.dd_i = Xist.dd_i(1:Nmod); | ||
+ | Xist.u = Xist.u(1:Nmod); | ||
+ | Xist.d_u = Xist.d_u(1:Nmod); | ||
+ | Xist.dd_u = Xist.dd_u(1:Nmod); | ||
+ | Xist.r = Xist.r(1:Nmod); | ||
+ | Xist.d_r = Xist.d_r(1:Nmod); | ||
+ | Xist.dd_r = Xist.dd_r(1:Nmod); | ||
+ | Xist.E = Xist.E(1:Nmod); | ||
+ | Xist.x0 = Xist.x0(1:Nmod); | ||
+ | Xist.y0 = Xist.y0(1:Nmod); | ||
+ | Xist.z0 = Xist.z0(1:Nmod); | ||
+ | Xist.theta1 = Xist.theta1(1:Nmod); | ||
+ | Xist.theta = Xist.theta(1:Nmod); | ||
+ | Xist.d_theta = Xist.d_theta(1:Nmod); | ||
+ | Xist.dd_theta = Xist.dd_theta(1:Nmod); | ||
+ | Xist.p = Xist.p(1:Nmod); | ||
+ | Xist.d_p = Xist.d_p(1:Nmod); | ||
+ | Xist.dd_p = Xist.dd_p(1:Nmod); | ||
+ | Xist.d_x0 = Xist.d_x0(1:Nmod); | ||
+ | Xist.d_y0 = Xist.d_y0(1:Nmod); | ||
+ | Xist.d_z0 = Xist.d_z0(1:Nmod); | ||
+ | Xist.x = Xist.x(1:Nmod); | ||
+ | Xist.y = Xist.y(1:Nmod); | ||
+ | Xist.z = Xist.z(1:Nmod); | ||
+ | |||
+ | |||
+ | % Estimations and extrapolations of Kalman | ||
+ | Xest.e = nan(1, Nmod); | ||
+ | Xest.d_e = nan(1, Nmod); | ||
+ | Xest.p = nan(1, Nmod); | ||
+ | Xest.d_p = nan(1, Nmod); | ||
+ | Xest.theta = nan(1, Nmod); | ||
+ | Xest.d_theta = nan(1, Nmod); | ||
+ | Xest.omega = nan(1, Nmod); | ||
+ | Xest.d_omega = nan(1, Nmod); | ||
+ | Xest.Omega = nan(1, Nmod); | ||
+ | Xest.d_Omega = nan(1, Nmod); | ||
+ | Xest.i = nan(1, Nmod); | ||
+ | Xest.d_i = nan(1, Nmod); | ||
+ | Xest.x0 = nan(1, Nmod); | ||
+ | Xest.y0 = nan(1, Nmod); | ||
+ | Xest.z0 = nan(1, Nmod); | ||
+ | Xest.d_x0 = nan(1, Nmod); | ||
+ | Xest.d_y0 = nan(1, Nmod); | ||
+ | Xest.d_z0 = nan(1, Nmod); | ||
+ | Xest.lambda = nan(1, Nmod); | ||
+ | Xest.x = nan(1, Nmod); | ||
+ | Xest.y = nan(1, Nmod); | ||
+ | Xest.z = nan(1, Nmod); | ||
+ | Xest.X = nan(12,1); | ||
+ | Xextr = Xest; % - extrapolation | ||
+ | |||
+ | % Without noise solution | ||
+ | Xest_won = Xest; | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===track_with_noise.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | %> ====================================================================== | ||
+ | %> @brief Tracking alghorithm | ||
+ | %> @param handles Main handles struct | ||
+ | %> ====================================================================== | ||
+ | function track_with_noise( handles, std_x, std_V ) | ||
+ | |||
+ | set(handles.pb_Track, 'Enable', 'off'); | ||
+ | pause(0.01); | ||
+ | |||
+ | globals; | ||
+ | |||
+ | % Filter step | ||
+ | T = dTmod; | ||
+ | |||
+ | % Evolutionary matrix | ||
+ | F = [1 T 0 0 0 0 0 0 0 0 0 0; | ||
+ | 0 1 0 0 0 0 0 0 0 0 0 0; | ||
+ | 0 0 1 T 0 0 0 0 0 0 0 0; | ||
+ | 0 0 0 1 0 0 0 0 0 0 0 0; | ||
+ | 0 0 0 0 1 T 0 0 0 0 0 0; | ||
+ | 0 0 0 0 0 1 0 0 0 0 0 0; | ||
+ | 0 0 0 0 0 0 1 T 0 0 0 0; | ||
+ | 0 0 0 0 0 0 0 1 0 0 0 0; | ||
+ | 0 0 0 0 0 0 0 0 1 T 0 0; | ||
+ | 0 0 0 0 0 0 0 0 0 1 0 0; | ||
+ | 0 0 0 0 0 0 0 0 0 0 1 T; | ||
+ | 0 0 0 0 0 0 0 0 0 0 0 1]; | ||
+ | |||
+ | p_mult = 5e7; % To simplify matrix calculations - reducing the dynamic range | ||
+ | |||
+ | % Xextr.X = [Xest_won.e(1); Xest_won.p(1)/p_mult; Xest_won.theta(1); 0; Xest_won.omega(1); 0; Xest_won.Omega(1); 0; Xest_won.i(1); 0]; | ||
+ | % Xextr.X = [0.01; 0; Xest_won.p(1)/p_mult; 0; 1; Xest_won.d_theta(1)*1.1; 0; 0; 0; Xest_won.d_Omega(1)*0.9; Xest_won.i(1)*0.9; Xest_won.d_i(1)]; | ||
+ | Xextr.X = [0.01; 0; 7e6/p_mult; 0; 1; Xest_won.d_theta(1)*1.1; 0; 0; 0; Xest_won.d_Omega(1)*0.9; Xest_won.i(1)*0.9; Xest_won.d_i(1)]; | ||
+ | Xest.X = Xextr.X; | ||
+ | |||
+ | % RMS of shaping noises | ||
+ | std_e = 5e-6 / 15*dTmod; | ||
+ | std_p = 1 / 15*dTmod / p_mult; % [m] | ||
+ | std_theta = 1e-5 / 15*dTmod; % [rad] | ||
+ | std_omega = 1e-6 / 15*dTmod; % [rad] | ||
+ | std_Omega = 1e-8 / 15*dTmod; % [rad] | ||
+ | std_i = 1e-8 / 15*dTmod; % [rad] | ||
+ | |||
+ | Dest = [std_e^2*1e1 0 0 0 0 0 0 0 0 0 0 0 | ||
+ | 0 std_e^2*1e2 0 0 0 0 0 0 0 0 0 0 | ||
+ | 0 0 std_p^2*1e3 0 0 0 0 0 0 0 0 0 | ||
+ | 0 0 0 std_p^2*1e4 0 0 0 0 0 0 0 0 | ||
+ | 0 0 0 0 std_theta^2*1e2 0 0 0 0 0 0 0 | ||
+ | 0 0 0 0 0 std_theta^2*1e0 0 0 0 0 0 0 | ||
+ | 0 0 0 0 0 0 std_omega^2*1e2 0 0 0 0 0 | ||
+ | 0 0 0 0 0 0 0 std_omega^2*1e2 0 0 0 0 | ||
+ | 0 0 0 0 0 0 0 0 std_Omega^2*1e2 0 0 0 | ||
+ | 0 0 0 0 0 0 0 0 0 std_Omega^2*1e2 0 0 | ||
+ | 0 0 0 0 0 0 0 0 0 0 std_i^2*1e2 0 | ||
+ | 0 0 0 0 0 0 0 0 0 0 0 std_i^2*1e2]; | ||
+ | |||
+ | G = [0 0 0 0 0 0; | ||
+ | 1 0 0 0 0 0; | ||
+ | 0 0 0 0 0 0; | ||
+ | 0 1 0 0 0 0; | ||
+ | 0 0 0 0 0 0; | ||
+ | 0 0 1 0 0 0; | ||
+ | 0 0 0 0 0 0; | ||
+ | 0 0 0 1 0 0; | ||
+ | 0 0 0 0 0 0; | ||
+ | 0 0 0 0 1 0; | ||
+ | 0 0 0 0 0 0; | ||
+ | 0 0 0 0 0 1]; | ||
+ | |||
+ | Dg = [std_e^2 0 0 0 0 0 | ||
+ | 0 std_p^2 0 0 0 0 | ||
+ | 0 0 std_theta^2 0 0 0 | ||
+ | 0 0 0 std_omega^2 0 0 | ||
+ | 0 0 0 0 std_Omega^2 0 | ||
+ | 0 0 0 0 0 std_i^2]; | ||
+ | |||
+ | GDgG = G*Dg*G'; | ||
+ | |||
+ | % std_x = 10 / sqrt(dTmod) ; | ||
+ | % std_V = 0.01 / sqrt(dTmod); | ||
+ | |||
+ | Dn = [std_x^2 0 0 0 0 0 | ||
+ | 0 std_x^2 0 0 0 0 | ||
+ | 0 0 std_x^2 0 0 0 | ||
+ | 0 0 0 std_V^2 0 0 | ||
+ | 0 0 0 0 std_V^2 0 | ||
+ | 0 0 0 0 0 std_V^2]; | ||
+ | |||
+ | c = [1 0 0 0 0 0 0 0 0 0 0 0; | ||
+ | 0 0 1 0 0 0 0 0 0 0 0 0; | ||
+ | 0 0 0 0 1 0 0 0 0 0 0 0; | ||
+ | 0 0 0 0 0 0 1 0 0 0 0 0; | ||
+ | 0 0 0 0 0 0 0 0 1 0 0 0; | ||
+ | 0 0 0 0 0 0 0 0 0 0 1 0]; | ||
+ | |||
+ | for i = 1:Nmod | ||
+ | |||
+ | x0_izm = Xist.x0(i) + std_x * randn(1,1); | ||
+ | y0_izm = Xist.y0(i) + std_x * randn(1,1); | ||
+ | z0_izm = Xist.z0(i) + std_x * randn(1,1); | ||
+ | Vx_izm = Xist.d_x0(i) + std_V * randn(1,1); | ||
+ | Vy_izm = Xist.d_y0(i) + std_V * randn(1,1); | ||
+ | Vz_izm = Xist.d_z0(i) + std_V * randn(1,1); | ||
+ | |||
+ | % For testing and debug | ||
+ | % Xextr.X(1) = Xest_won.e(i); | ||
+ | % Xextr.X(3) = Xest_won.p(i)/p_mult; | ||
+ | % Xextr.X(5) = Xest_won.theta(i); | ||
+ | % Xextr.X(7) = Xest_won.omega(i); | ||
+ | % Xextr.X(9) = Xest_won.Omega(i); | ||
+ | % Xextr.X(11) = Xest_won.i(i); | ||
+ | |||
+ | e = Xextr.X(1); | ||
+ | p = Xextr.X(3) * p_mult; | ||
+ | theta = Xextr.X(5); | ||
+ | omega = Xextr.X(7); | ||
+ | Omega = Xextr.X(9); | ||
+ | i0 = Xextr.X(11); | ||
+ | |||
+ | munapi = sqrt(mu_earth / p); | ||
+ | u = theta + omega; | ||
+ | |||
+ | oec = 1 + e*cos(theta); | ||
+ | es = e*sin(theta); | ||
+ | |||
+ | Sc_x = cos(u)*cos(Omega) - sin(u)*sin(Omega)*cos(i0); | ||
+ | Sc_y = cos(u)*sin(Omega) + sin(u)*cos(Omega)*cos(i0); | ||
+ | Sc_z = sin(u)*sin(i0); | ||
+ | |||
+ | Sc_Vx = sin(u)*cos(Omega) + cos(u)*sin(Omega)*cos(i0); | ||
+ | Sc_Vy = sin(u)*sin(Omega) - cos(u)*cos(Omega)*cos(i0); | ||
+ | Sc_Vz = cos(u)*sin(i0); | ||
+ | |||
+ | [x0_extr y0_extr z0_extr Vx_extr Vy_extr Vz_extr] = ... | ||
+ | get_vector_XV( e, p, theta, omega, Omega, i0); | ||
+ | |||
+ | dY = [x0_izm; y0_izm; z0_izm; Vx_izm; Vy_izm; Vz_izm] - ... | ||
+ | [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr]; | ||
+ | |||
+ | S0 = nan(6,6); | ||
+ | |||
+ | % Discriminator for e | ||
+ | S0(1, 1) = - p * Sc_x / (1+e*cos(theta))^2 * cos(theta); | ||
+ | S0(2, 1) = - p * Sc_y / (1+e*cos(theta))^2 * cos(theta); | ||
+ | S0(3, 1) = - p * Sc_z / (1+e*cos(theta))^2 * cos(theta); | ||
+ | S0(4, 1) = munapi * (sin(theta)*Sc_x - cos(theta)*Sc_Vx); | ||
+ | S0(5, 1) = munapi * (sin(theta)*Sc_y - cos(theta)*Sc_Vy); | ||
+ | S0(6, 1) = munapi * (sin(theta)*Sc_z + cos(theta)*Sc_Vz); | ||
+ | |||
+ | % Discriminator for p | ||
+ | S0(1, 2) = Sc_x/oec; | ||
+ | S0(2, 2) = Sc_y/oec; | ||
+ | S0(3, 2) = Sc_z/oec; | ||
+ | S0(4, 2) = - 0.5 * 1/p * munapi * (es *Sc_x - oec*Sc_Vx); | ||
+ | S0(5, 2) = - 0.5 * 1/p * munapi * (es *Sc_y - oec*Sc_Vy); | ||
+ | S0(6, 2) = - 0.5 * 1/p * munapi * (es *Sc_z + oec*Sc_Vz); | ||
+ | S0(:,2) = S0(:,2) * p_mult; | ||
+ | |||
+ | % Discriminator for theta | ||
+ | Sc_x_theta = - sin(u)*cos(Omega) - cos(u)*sin(Omega)*cos(i0); | ||
+ | Sc_y_theta = - sin(u)*sin(Omega) + cos(u)*cos(Omega)*cos(i0); | ||
+ | Sc_z_theta = cos(u)*sin(i0); | ||
+ | Sc_Vx_theta = cos(u)*cos(Omega) - sin(u)*sin(Omega)*cos(i0); | ||
+ | Sc_Vy_theta = cos(u)*sin(Omega) + sin(u)*cos(Omega)*cos(i0); | ||
+ | Sc_Vz_theta = -sin(u)*sin(i0); | ||
+ | |||
+ | S0(1, 3) = e*Sc_x*p / oec^2 * sin(theta) + p / oec * Sc_x_theta; | ||
+ | S0(2, 3) = e*Sc_y*p / oec^2 * sin(theta) + p / oec * Sc_y_theta; | ||
+ | S0(3, 3) = e*Sc_z*p / oec^2 * sin(theta) + p / oec * Sc_z_theta; | ||
+ | S0(4, 3) = -munapi*Sc_Vx_theta; | ||
+ | S0(5, 3) = -munapi*Sc_Vy_theta; | ||
+ | S0(6, 3) = munapi*Sc_Vz_theta; | ||
+ | |||
+ | % dx = 0.001; | ||
+ | % [x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ... | ||
+ | % get_vector_XV( e, p, theta+dx, omega, Omega, i0); | ||
+ | % dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ... | ||
+ | % [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr]; | ||
+ | % | ||
+ | % S0(1, 3) = dS(1) / dx; | ||
+ | % S0(2, 3) = dS(2) / dx; | ||
+ | % S0(3, 3) = dS(3) / dx; | ||
+ | % S0(4, 3) = dS(4) / dx; | ||
+ | % S0(5, 3) = dS(5) / dx; | ||
+ | % S0(6, 3) = dS(6) / dx; | ||
+ | |||
+ | % Discriminator for omega | ||
+ | Sc_x_omega = Sc_x_theta; | ||
+ | Sc_y_omega = Sc_y_theta; | ||
+ | Sc_z_omega = Sc_z_theta; | ||
+ | Sc_Vx_omega = Sc_Vx_theta; | ||
+ | Sc_Vy_omega = Sc_Vy_theta; | ||
+ | Sc_Vz_omega = Sc_Vz_theta; | ||
+ | |||
+ | S0(1,4) = p / oec * Sc_x_omega; | ||
+ | S0(2,4) = p / oec * Sc_y_omega; | ||
+ | S0(3,4) = p / oec * Sc_z_omega; | ||
+ | S0(4,4) = munapi * (e*sin(omega)*Sc_x_omega - oec*Sc_Vx_omega); | ||
+ | S0(5,4) = munapi * (e*sin(omega)*Sc_y_omega - oec*Sc_Vy_omega); | ||
+ | S0(6,4) = munapi * (e*sin(omega)*Sc_z_omega + oec*Sc_Vz_omega); | ||
+ | |||
+ | S01(i) = S0(1,4); | ||
+ | S02(i) = S0(2,4); | ||
+ | S03(i) = S0(3,4); | ||
+ | S04(i) = S0(4,4); | ||
+ | S05(i) = S0(5,4); | ||
+ | S06(i) = S0(6,4); | ||
+ | |||
+ | dx = 1e-6; | ||
+ | [x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ... | ||
+ | get_vector_XV( e, p, theta, omega+dx, Omega, i0); | ||
+ | dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ... | ||
+ | [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr]; | ||
+ | |||
+ | S0(1, 4) = dS(1) / dx; | ||
+ | S0(2, 4) = dS(2) / dx; | ||
+ | S0(3, 4) = dS(3) / dx; | ||
+ | S0(4, 4) = dS(4) / dx; | ||
+ | S0(5, 4) = dS(5) / dx; | ||
+ | S0(6, 4) = dS(6) / dx; | ||
+ | |||
+ | S01_ist(i) = S0(1,4); | ||
+ | S02_ist(i) = S0(2,4); | ||
+ | S03_ist(i) = S0(3,4); | ||
+ | S04_ist(i) = S0(4,4); | ||
+ | S05_ist(i) = S0(5,4); | ||
+ | S06_ist(i) = S0(6,4); | ||
+ | |||
+ | |||
+ | % Discriminator for Omega | ||
+ | Sc_x_Omega = -cos(u)*sin(Omega) - sin(u)*cos(Omega)*cos(i0); | ||
+ | Sc_y_Omega = cos(u)*cos(Omega) - sin(u)*sin(Omega)*cos(i0); | ||
+ | Sc_z_Omega = 0; | ||
+ | Sc_Vx_Omega = -sin(u)*sin(Omega) + cos(u)*cos(Omega)*cos(i0); | ||
+ | Sc_Vy_Omega = sin(u)*cos(Omega) + cos(u)*sin(Omega)*cos(i0); | ||
+ | Sc_Vz_Omega = 0; | ||
+ | |||
+ | S0(1,5) = p/oec*Sc_x_Omega; | ||
+ | S0(2,5) = p/oec*Sc_y_Omega; | ||
+ | S0(3,5) = 0; | ||
+ | S0(4,5) = munapi * (e*sin(omega)*Sc_x_Omega - oec*Sc_Vx_Omega); | ||
+ | S0(5,5) = munapi * (e*sin(omega)*Sc_y_Omega - oec*Sc_Vy_Omega); | ||
+ | S0(6,5) = 0; | ||
+ | |||
+ | % dx = 0.001; | ||
+ | % [x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ... | ||
+ | % get_vector_XV( e, p, theta, omega, Omega+dx, i0); | ||
+ | % dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ... | ||
+ | % [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr]; | ||
+ | % | ||
+ | % S0(1, 5) = dS(1) / dx; | ||
+ | % S0(2, 5) = dS(2) / dx; | ||
+ | % S0(3, 5) = dS(3) / dx; | ||
+ | % S0(4, 5) = dS(4) / dx; | ||
+ | % S0(5, 5) = dS(5) / dx; | ||
+ | % S0(6, 5) = dS(6) / dx; | ||
+ | |||
+ | % Discriminator for i | ||
+ | Sc_x_i = sin(u)*sin(Omega)*sin(i0); | ||
+ | Sc_y_i = -sin(u)*cos(Omega)*sin(i0); | ||
+ | Sc_z_i = sin(u)*cos(i0); | ||
+ | Sc_Vx_i = -cos(u)*sin(Omega)*sin(i0); | ||
+ | Sc_Vy_i = cos(u)*cos(Omega)*sin(i0); | ||
+ | Sc_Vz_i = cos(u)*cos(i0); | ||
+ | |||
+ | S0(1, 6) = p/oec * Sc_x_i; | ||
+ | S0(2, 6) = p/oec * Sc_y_i; | ||
+ | S0(3, 6) = p/oec * Sc_z_i; | ||
+ | S0(4, 6) = munapi * (e*sin(theta)*Sc_x_i - oec*Sc_Vx_i); | ||
+ | S0(5, 6) = munapi * (e*sin(theta)*Sc_y_i - oec*Sc_Vy_i); | ||
+ | S0(6, 6) = munapi * (e*sin(theta)*Sc_z_i + oec*Sc_Vz_i); | ||
+ | |||
+ | % dx = 0.001; | ||
+ | % [x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ... | ||
+ | % get_vector_XV( e, p, theta, omega, Omega, i0+dx); | ||
+ | % dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ... | ||
+ | % [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr]; | ||
+ | % | ||
+ | % S0(1, 6) = dS(1) / dx; | ||
+ | % S0(2, 6) = dS(2) / dx; | ||
+ | % S0(3, 6) = dS(3) / dx; | ||
+ | % S0(4, 6) = dS(4) / dx; | ||
+ | % S0(5, 6) = dS(5) / dx; | ||
+ | % S0(6, 6) = dS(6) / dx; | ||
+ | |||
+ | S = (c'*S0')'; | ||
+ | |||
+ | Dextr = F*Dest*F' + GDgG; | ||
+ | t1 = S'/Dn*S; | ||
+ | t2 = inv(Dextr); | ||
+ | Dest = inv(t1 + t2); | ||
+ | |||
+ | Xest.X = Xextr.X + Dest*S'/Dn*dY; | ||
+ | Xextr.X = F*Xest.X; | ||
+ | |||
+ | if Xest.X(1) > 0; | ||
+ | Xest.e(i) = Xest.X(1); | ||
+ | Xest.theta(i) = mod_pm_pi(Xest.X(5)); | ||
+ | Xest.omega(i) = mod_pm_pi(Xest.X(7)); | ||
+ | else | ||
+ | Xest.e(i) = -Xest.X(1); | ||
+ | Xest.theta(i) = mod_pm_pi(-Xest.X(5)); | ||
+ | Xest.omega(i) = mod_pm_pi(-Xest.X(7)); | ||
+ | end | ||
+ | Xest.p(i) = Xest.X(3)*p_mult; | ||
+ | Xest.Omega(i) = mod_pm_pi(Xest.X(9)); | ||
+ | Xest.i(i) = mod_pm_pi(Xest.X(11)); | ||
+ | |||
+ | [Xest.x0(i) Xest.y0(i) Xest.z0(i) Xest.d_x0(i) Xest.d_y0(i) Xest.d_z0(i)] = ... | ||
+ | get_vector_XV( Xest.e(i), Xest.p(i), Xest.theta(i), Xest.omega(i), Xest.Omega(i), Xest.i(i) ); | ||
+ | [Xest.x(i) Xest.y(i) Xest.z(i) tmp1 tmp2 tmp3] = ... | ||
+ | get_vector_XV( Xest.e(i), Xest.p(i), Xest.theta(i), Xest.omega(i), Xest.Omega(i) - omega_e*tmod(i), Xest.i(i) ); | ||
+ | |||
+ | Xest.d_e(i) = Xest.X(2); | ||
+ | Xest.d_p(i) = Xest.X(4)*p_mult; | ||
+ | Xest.d_theta(i) = Xest.X(6); | ||
+ | Xest.d_omega(i) = Xest.X(8); | ||
+ | Xest.d_Omega(i) = Xest.X(10); | ||
+ | Xest.d_i(i) = Xest.X(12); | ||
+ | |||
+ | if ~mod(i, fix(Nmod/100)) | ||
+ | set(handles.txt_Track, 'String', sprintf('%.0f %%', i/Nmod*100)); | ||
+ | end | ||
+ | drawnow | ||
+ | pause(0.01); | ||
+ | end | ||
+ | set(handles.txt_Track, 'String', sprintf('%.0f %%', 100)); | ||
+ | |||
+ | set(handles.pb_Track, 'Enable', 'on'); | ||
+ | |||
+ | set(handles.pan_Orbital_Elements, 'Visible', 'off'); | ||
+ | set(handles.pan_Tracking, 'Visible', 'on'); | ||
+ | |||
+ | % Output results to form | ||
+ | for i = 1:5 | ||
+ | for j = 1:5 | ||
+ | set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'DrawMode', 'fast'); | ||
+ | plot_axes_Track(handles, 0, [num2str(i) num2str(j)]); | ||
+ | set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Tag', ['axes_Track_' num2str(i) num2str(j)]); | ||
+ | set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_Track_ButtonDownFcn'',hObject,eventdata,guidata(hObject))')); | ||
+ | end | ||
+ | end | ||
+ | plot_axes_Earth(handles, 0); | ||
+ | draw_Errors(handles); | ||
+ | |||
+ | end | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===U1.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | function M = U1(x) | ||
+ | |||
+ | M = [1 0 0; | ||
+ | 0 cos(x) sin(x); | ||
+ | 0 -sin(x) cos(x)]; | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===U2.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | function M = U2(x) | ||
+ | |||
+ | M = [cos(x) 0 -sin(x); | ||
+ | 0 1 0; | ||
+ | sin(x) 0 cos(x)]; | ||
+ | </source> | ||
+ | |||
+ | |||
+ | ===U3.m=== | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | function M = U3(x) | ||
+ | |||
+ | M = [cos(x) sin(x) 0; | ||
+ | -sin(x) cos(x) 0; | ||
+ | 0 0 1]; | ||
+ | </source> |
Текущая версия на 12:47, 29 июня 2012
[править] main.m
try
close(handle_fig_main); % Close old output form
end
close all
clear
clc
globals;
addpath([pwd '/func/tracking']); % Functions for tracking algorithm
addpath([pwd '/func/solve']); % Functions for solving without noise
addpath([pwd '/func/interface']); % Functions for interface
Tmod = 3.2*60*60; %[s], duration of the simulation
% Magic constants
hF_cont = 0; % Last figure's handles
Font_Size = 8; % Font size for output interface
mu_earth = 3.9860044e14; % [m^3/s^2] Gravity constant
omega_e = 0.7292115e-4; % [rad/s] Earth's rotation rate
options_solve = optimset('Display','off'); % Turn off display for fsolve
% Load true trajectory of SV
load TrueTrajectory.mat
Nmod = fix(Tmod/T);
if Nmod > Nmod_max
Nmod = Nmod_max;
Tmod = (Nmod-1)*T;
end
resize_arrays; % resize arrays for new Tmod
handle_fig_main = fig_main(); % open GUI form
close(handle_fig_main); % Close old output form
end
close all
clear
clc
globals;
addpath([pwd '/func/tracking']); % Functions for tracking algorithm
addpath([pwd '/func/solve']); % Functions for solving without noise
addpath([pwd '/func/interface']); % Functions for interface
Tmod = 3.2*60*60; %[s], duration of the simulation
% Magic constants
hF_cont = 0; % Last figure's handles
Font_Size = 8; % Font size for output interface
mu_earth = 3.9860044e14; % [m^3/s^2] Gravity constant
omega_e = 0.7292115e-4; % [rad/s] Earth's rotation rate
options_solve = optimset('Display','off'); % Turn off display for fsolve
% Load true trajectory of SV
load TrueTrajectory.mat
Nmod = fix(Tmod/T);
if Nmod > Nmod_max
Nmod = Nmod_max;
Tmod = (Nmod-1)*T;
end
resize_arrays; % resize arrays for new Tmod
handle_fig_main = fig_main(); % open GUI form
[править] globals.m
global Font_Size mu_earth omega_e
global SV_GLO_List SV_GLO
global Xist Xextr Xest Xest_won
global Tmod dTmod tmod Nmod
global start_handle hF_cont
global options_solve
global SV_GLO_List SV_GLO
global Xist Xextr Xest Xest_won
global Tmod dTmod tmod Nmod
global start_handle hF_cont
global options_solve
[править] fig_main.m
function varargout = fig_main(varargin)
% FIG_MAIN M-file for fig_main.fig
% FIG_MAIN, by itself, creates a new FIG_MAIN or raises the existing
% singleton*.
%
% H = FIG_MAIN returns the handle to a new FIG_MAIN or the handle to
% the existing singleton*.
%
% FIG_MAIN('CALLBACK',hObject,eventData,handles,...) calls the local
% function named CALLBACK in FIG_MAIN.M with the given input arguments.
%
% FIG_MAIN('Property','Value',...) creates a new FIG_MAIN or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before fig_main_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property application
% stop. All inputs are passed to fig_main_OpeningFcn via varargin.
%
% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
% instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help fig_main
% Last Modified by GUIDE v2.5 13-Jun-2012 12:18:01
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @fig_main_OpeningFcn, ...
'gui_OutputFcn', @fig_main_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT
% --- Executes just before fig_main is made visible.
function fig_main_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% varargin command line arguments to fig_main (see VARARGIN)
% Choose default command line output for fig_main
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
set(handles.fig_main, 'Units', 'pixels');
set(0, 'Units', 'pixels');
% Locate form
ScreenSize = get(0,'ScreenSize');
if ((ScreenSize(3) < 1280)||(ScreenSize(4) < 720))
msgbox('Sceeen size too small!');
close(handles.fig_main);
end
FigWeigth = 1250;
FigWeigth_border = fix((ScreenSize(3) - FigWeigth)/2);
FigHeigth = 670;
FigHeigth_border = fix((ScreenSize(4) - FigHeigth)/2);
set(handles.fig_main, 'Position', [FigWeigth_border FigHeigth_border FigWeigth FigHeigth]);
set(handles.pan_Tracking, 'Position', get(handles.pan_Orbital_Elements, 'Position'));
set(handles.pan_Sch, 'Position', get(handles.pan_Orbital_Elements, 'Position'));
% Draw functional scheme
fig_main_pictureData = imread('Sch1.png');
image(fig_main_pictureData, 'Parent', handles.axes_Sch1);
set(handles.axes_Sch1, 'XTick', []);
set(handles.axes_Sch1, 'YTick', []);
% Set widget's function
for i = 1:5
for j = 1:5
set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'DrawMode', 'fast');
plot_axes_OE(handles, 0, [num2str(i) num2str(j)]);
set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'Tag', ['axes_OE_' num2str(i) num2str(j)]);
set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_OE_ButtonDownFcn'',hObject,eventdata,guidata(hObject))'));
pos = get(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'Position');
pos(1) = pos(1) + (j-1)*45;
pos(2) = pos(2) - (i-1)*32 - 35;
pos(3) = pos(3) + 40;
pos(4) = pos(4) + 35;
set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'Position', pos);
end
end
for i = 1:5
for j = 1:5
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'DrawMode', 'fast');
plot_axes_Track(handles, 0, [num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Tag', ['axes_Track_' num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_Track_ButtonDownFcn'',hObject,eventdata,guidata(hObject))'));
pos = get(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Position');
pos(1) = pos(1) + (j-1)*45;
pos(2) = pos(2) - (i-1)*32 - 35;
pos(3) = pos(3) + 40;
pos(4) = pos(4) + 35;
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Position', pos);
end
end
plot_axes_Earth(handles, 0);
globals;
% Set constants
start_handle = handles;
% UIWAIT makes fig_main wait for user response (see UIRESUME)
% uiwait(handles.fig_main);
% --- Outputs from this function are returned to the command line.
function varargout = fig_main_OutputFcn(hObject, eventdata, handles)
% varargout cell array for returning output args (see VARARGOUT);
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Get default command line output from handles structure
varargout{1} = handles.output;
% --- Executes on mouse press over axes background.
function axes_3D_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_3D (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
globals;
plot_axes_Earth(handles, next_hF());
% --- Executes on mouse press over axes background.
function axes_OE_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_OE_ (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
dig = get(hObject, 'Tag'); dig = dig(end-1:end);
plot_axes_OE(handles, next_hF(), dig)
% --- Executes on mouse press over axes background.
function axes_Track_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_OE_ (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
dig = get(hObject, 'Tag'); dig = dig(end-1:end);
plot_axes_Track(handles, next_hF(), dig)
% --- Executes on button press in pb_Track.
function pb_Track_Callback(hObject, eventdata, handles)
% hObject handle to pb_Track (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
std_x = str2double(get(handles.ed_stdX, 'String'));
if ~((std_x >= 0)&&(std_x < 1000))
msgbox('Coordinate''s RMS is incorrect', 'Error', 'error');
set(handles.ed_stdX, 'String', '10');
return
end
std_V = str2double(get(handles.ed_stdV, 'String'));
if ~((std_V >= 0)&&(std_V < 5))
msgbox('Velocity''s RMS is incorrect', 'Error', 'error');
set(handles.ed_stdV, 'String', '0.01');
return
end
track_with_noise(handles, std_x, std_V);
% --- Executes on button press in pb_True.
function pb_True_Callback(hObject, eventdata, handles)
% hObject handle to pb_True (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
set(handles.pan_Orbital_Elements, 'Visible', 'on');
set(handles.pan_Tracking, 'Visible', 'off');
set(handles.pan_Sch, 'Visible', 'off');
% --- Executes on button press in pb_Tracking.
function pb_Tracking_Callback(hObject, eventdata, handles)
% hObject handle to pb_Tracking (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
set(handles.pan_Orbital_Elements, 'Visible', 'off');
set(handles.pan_Tracking, 'Visible', 'on');
set(handles.pan_Sch, 'Visible', 'off');
% --- Executes on button press in pb_Solve.
function pb_Solve_Callback(hObject, eventdata, handles)
% hObject handle to pb_Solve (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
solve_wo_noise(handles);
function ed_stdX_Callback(hObject, eventdata, handles)
% hObject handle to ed_stdX (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of ed_stdX as text
% str2double(get(hObject,'String')) returns contents of ed_stdX as a double
% --- Executes during object creation, after setting all properties.
function ed_stdX_CreateFcn(hObject, eventdata, handles)
% hObject handle to ed_stdX (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function ed_stdV_Callback(hObject, eventdata, handles)
% hObject handle to ed_stdV (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of ed_stdV as text
% str2double(get(hObject,'String')) returns contents of ed_stdV as a double
% --- Executes during object creation, after setting all properties.
function ed_stdV_CreateFcn(hObject, eventdata, handles)
% hObject handle to ed_stdV (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
% --- Executes on button press in pb_Sch.
function pb_Sch_Callback(hObject, eventdata, handles)
% hObject handle to pb_Sch (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
set(handles.pan_Orbital_Elements, 'Visible', 'off');
set(handles.pan_Tracking, 'Visible', 'off');
set(handles.pan_Sch, 'Visible', 'on');
% FIG_MAIN M-file for fig_main.fig
% FIG_MAIN, by itself, creates a new FIG_MAIN or raises the existing
% singleton*.
%
% H = FIG_MAIN returns the handle to a new FIG_MAIN or the handle to
% the existing singleton*.
%
% FIG_MAIN('CALLBACK',hObject,eventData,handles,...) calls the local
% function named CALLBACK in FIG_MAIN.M with the given input arguments.
%
% FIG_MAIN('Property','Value',...) creates a new FIG_MAIN or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before fig_main_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property application
% stop. All inputs are passed to fig_main_OpeningFcn via varargin.
%
% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
% instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help fig_main
% Last Modified by GUIDE v2.5 13-Jun-2012 12:18:01
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @fig_main_OpeningFcn, ...
'gui_OutputFcn', @fig_main_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT
% --- Executes just before fig_main is made visible.
function fig_main_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% varargin command line arguments to fig_main (see VARARGIN)
% Choose default command line output for fig_main
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
set(handles.fig_main, 'Units', 'pixels');
set(0, 'Units', 'pixels');
% Locate form
ScreenSize = get(0,'ScreenSize');
if ((ScreenSize(3) < 1280)||(ScreenSize(4) < 720))
msgbox('Sceeen size too small!');
close(handles.fig_main);
end
FigWeigth = 1250;
FigWeigth_border = fix((ScreenSize(3) - FigWeigth)/2);
FigHeigth = 670;
FigHeigth_border = fix((ScreenSize(4) - FigHeigth)/2);
set(handles.fig_main, 'Position', [FigWeigth_border FigHeigth_border FigWeigth FigHeigth]);
set(handles.pan_Tracking, 'Position', get(handles.pan_Orbital_Elements, 'Position'));
set(handles.pan_Sch, 'Position', get(handles.pan_Orbital_Elements, 'Position'));
% Draw functional scheme
fig_main_pictureData = imread('Sch1.png');
image(fig_main_pictureData, 'Parent', handles.axes_Sch1);
set(handles.axes_Sch1, 'XTick', []);
set(handles.axes_Sch1, 'YTick', []);
% Set widget's function
for i = 1:5
for j = 1:5
set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'DrawMode', 'fast');
plot_axes_OE(handles, 0, [num2str(i) num2str(j)]);
set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'Tag', ['axes_OE_' num2str(i) num2str(j)]);
set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_OE_ButtonDownFcn'',hObject,eventdata,guidata(hObject))'));
pos = get(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'Position');
pos(1) = pos(1) + (j-1)*45;
pos(2) = pos(2) - (i-1)*32 - 35;
pos(3) = pos(3) + 40;
pos(4) = pos(4) + 35;
set(eval(['handles.axes_OE_' num2str(i) num2str(j)]), 'Position', pos);
end
end
for i = 1:5
for j = 1:5
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'DrawMode', 'fast');
plot_axes_Track(handles, 0, [num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Tag', ['axes_Track_' num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_Track_ButtonDownFcn'',hObject,eventdata,guidata(hObject))'));
pos = get(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Position');
pos(1) = pos(1) + (j-1)*45;
pos(2) = pos(2) - (i-1)*32 - 35;
pos(3) = pos(3) + 40;
pos(4) = pos(4) + 35;
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Position', pos);
end
end
plot_axes_Earth(handles, 0);
globals;
% Set constants
start_handle = handles;
% UIWAIT makes fig_main wait for user response (see UIRESUME)
% uiwait(handles.fig_main);
% --- Outputs from this function are returned to the command line.
function varargout = fig_main_OutputFcn(hObject, eventdata, handles)
% varargout cell array for returning output args (see VARARGOUT);
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Get default command line output from handles structure
varargout{1} = handles.output;
% --- Executes on mouse press over axes background.
function axes_3D_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_3D (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
globals;
plot_axes_Earth(handles, next_hF());
% --- Executes on mouse press over axes background.
function axes_OE_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_OE_ (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
dig = get(hObject, 'Tag'); dig = dig(end-1:end);
plot_axes_OE(handles, next_hF(), dig)
% --- Executes on mouse press over axes background.
function axes_Track_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_OE_ (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
dig = get(hObject, 'Tag'); dig = dig(end-1:end);
plot_axes_Track(handles, next_hF(), dig)
% --- Executes on button press in pb_Track.
function pb_Track_Callback(hObject, eventdata, handles)
% hObject handle to pb_Track (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
std_x = str2double(get(handles.ed_stdX, 'String'));
if ~((std_x >= 0)&&(std_x < 1000))
msgbox('Coordinate''s RMS is incorrect', 'Error', 'error');
set(handles.ed_stdX, 'String', '10');
return
end
std_V = str2double(get(handles.ed_stdV, 'String'));
if ~((std_V >= 0)&&(std_V < 5))
msgbox('Velocity''s RMS is incorrect', 'Error', 'error');
set(handles.ed_stdV, 'String', '0.01');
return
end
track_with_noise(handles, std_x, std_V);
% --- Executes on button press in pb_True.
function pb_True_Callback(hObject, eventdata, handles)
% hObject handle to pb_True (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
set(handles.pan_Orbital_Elements, 'Visible', 'on');
set(handles.pan_Tracking, 'Visible', 'off');
set(handles.pan_Sch, 'Visible', 'off');
% --- Executes on button press in pb_Tracking.
function pb_Tracking_Callback(hObject, eventdata, handles)
% hObject handle to pb_Tracking (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
set(handles.pan_Orbital_Elements, 'Visible', 'off');
set(handles.pan_Tracking, 'Visible', 'on');
set(handles.pan_Sch, 'Visible', 'off');
% --- Executes on button press in pb_Solve.
function pb_Solve_Callback(hObject, eventdata, handles)
% hObject handle to pb_Solve (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
solve_wo_noise(handles);
function ed_stdX_Callback(hObject, eventdata, handles)
% hObject handle to ed_stdX (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of ed_stdX as text
% str2double(get(hObject,'String')) returns contents of ed_stdX as a double
% --- Executes during object creation, after setting all properties.
function ed_stdX_CreateFcn(hObject, eventdata, handles)
% hObject handle to ed_stdX (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function ed_stdV_Callback(hObject, eventdata, handles)
% hObject handle to ed_stdV (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of ed_stdV as text
% str2double(get(hObject,'String')) returns contents of ed_stdV as a double
% --- Executes during object creation, after setting all properties.
function ed_stdV_CreateFcn(hObject, eventdata, handles)
% hObject handle to ed_stdV (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
% --- Executes on button press in pb_Sch.
function pb_Sch_Callback(hObject, eventdata, handles)
% hObject handle to pb_Sch (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
set(handles.pan_Orbital_Elements, 'Visible', 'off');
set(handles.pan_Tracking, 'Visible', 'off');
set(handles.pan_Sch, 'Visible', 'on');
[править] drawErrors.m
%> ======================================================================
%> @brief plot Caclulate and draw RMSes
%> @param handles Main handles struct
% ======================================================================
function draw_Errors(handles)
globals;
begin_i = Nmod - 300; end_i = Nmod;
set(handles.txt_Err_11, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.e(begin_i:end_i) - Xist.e(begin_i:end_i)).^2)))));
set(handles.txt_Err_12, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_e(begin_i:end_i) - Xist.d_e(begin_i:end_i)).^2)))));
set(handles.txt_Err_21, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.p(begin_i:end_i) - Xist.p(begin_i:end_i)).^2)))));
set(handles.txt_Err_22, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_p(begin_i:end_i) - Xist.d_p(begin_i:end_i)).^2)))));
set(handles.txt_Err_31, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.theta(begin_i:end_i) - Xist.theta(begin_i:end_i)).^2)))));
set(handles.txt_Err_32, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_theta(begin_i:end_i) - Xist.d_theta(begin_i:end_i)).^2)))));
set(handles.txt_Err_41, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.omega(begin_i:end_i) - Xist.omega(begin_i:end_i)).^2)))));
set(handles.txt_Err_42, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_omega(begin_i:end_i) - Xist.d_omega(begin_i:end_i)).^2)))));
set(handles.txt_Err_51, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.Omega(begin_i:end_i) - Xist.Omega(begin_i:end_i)).^2)))));
set(handles.txt_Err_52, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_Omega(begin_i:end_i) - Xist.d_Omega(begin_i:end_i)).^2)))));
set(handles.txt_Err_61, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.i(begin_i:end_i) - Xist.i(begin_i:end_i)).^2)))));
set(handles.txt_Err_62, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_i(begin_i:end_i) - Xist.d_i(begin_i:end_i)).^2)))));
set(handles.txt_Err_71, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.x0(begin_i:end_i) - Xist.x0(begin_i:end_i)).^2)))));
set(handles.txt_Err_72, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_x0(begin_i:end_i) - Xist.d_x0(begin_i:end_i)).^2)))));
set(handles.txt_Err_81, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.y0(begin_i:end_i) - Xist.y0(begin_i:end_i)).^2)))));
set(handles.txt_Err_82, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_y0(begin_i:end_i) - Xist.d_y0(begin_i:end_i)).^2)))));
set(handles.txt_Err_91, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.z0(begin_i:end_i) - Xist.z0(begin_i:end_i)).^2)))));
set(handles.txt_Err_92, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_z0(begin_i:end_i) - Xist.d_z0(begin_i:end_i)).^2)))));
end
%> @brief plot Caclulate and draw RMSes
%> @param handles Main handles struct
% ======================================================================
function draw_Errors(handles)
globals;
begin_i = Nmod - 300; end_i = Nmod;
set(handles.txt_Err_11, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.e(begin_i:end_i) - Xist.e(begin_i:end_i)).^2)))));
set(handles.txt_Err_12, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_e(begin_i:end_i) - Xist.d_e(begin_i:end_i)).^2)))));
set(handles.txt_Err_21, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.p(begin_i:end_i) - Xist.p(begin_i:end_i)).^2)))));
set(handles.txt_Err_22, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_p(begin_i:end_i) - Xist.d_p(begin_i:end_i)).^2)))));
set(handles.txt_Err_31, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.theta(begin_i:end_i) - Xist.theta(begin_i:end_i)).^2)))));
set(handles.txt_Err_32, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_theta(begin_i:end_i) - Xist.d_theta(begin_i:end_i)).^2)))));
set(handles.txt_Err_41, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.omega(begin_i:end_i) - Xist.omega(begin_i:end_i)).^2)))));
set(handles.txt_Err_42, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_omega(begin_i:end_i) - Xist.d_omega(begin_i:end_i)).^2)))));
set(handles.txt_Err_51, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.Omega(begin_i:end_i) - Xist.Omega(begin_i:end_i)).^2)))));
set(handles.txt_Err_52, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_Omega(begin_i:end_i) - Xist.d_Omega(begin_i:end_i)).^2)))));
set(handles.txt_Err_61, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.i(begin_i:end_i) - Xist.i(begin_i:end_i)).^2)))));
set(handles.txt_Err_62, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_i(begin_i:end_i) - Xist.d_i(begin_i:end_i)).^2)))));
set(handles.txt_Err_71, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.x0(begin_i:end_i) - Xist.x0(begin_i:end_i)).^2)))));
set(handles.txt_Err_72, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_x0(begin_i:end_i) - Xist.d_x0(begin_i:end_i)).^2)))));
set(handles.txt_Err_81, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.y0(begin_i:end_i) - Xist.y0(begin_i:end_i)).^2)))));
set(handles.txt_Err_82, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_y0(begin_i:end_i) - Xist.d_y0(begin_i:end_i)).^2)))));
set(handles.txt_Err_91, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.z0(begin_i:end_i) - Xist.z0(begin_i:end_i)).^2)))));
set(handles.txt_Err_92, 'String', num2str(sprintf('%.1E', mean(sqrt((Xest.d_z0(begin_i:end_i) - Xist.d_z0(begin_i:end_i)).^2)))));
end
[править] next_hF.m
function hF_cont = next_hF()
global hF_cont
hF_cont = hF_cont + 1;
end
global hF_cont
hF_cont = hF_cont + 1;
end
[править] plot_axes_Earth.m
%> ======================================================================
%> @brief 3D-Earth, SV's orbits
%> @param handles Main handles struct
%> @param hF Flag (handle) for separate window
% ======================================================================
function plot_axes_Earth(handles, hF)
globals;
R_earth_pol = 6356777; % Polar radius of Earth
R_earth_equa = 6378160; % Equatorial radius of Earth
Earth_axe_angl = deg2rad(23);
[x,y,z] = sphere(50);
x = R_earth_equa * x; y = R_earth_equa * y; z = R_earth_pol * z;
load('topo.mat','topo','topomap1');
% cla reset
% axis square off
props.AmbientStrength = 0.1;
props.DiffuseStrength = 1;
props.SpecularColorReflectance = .5;
props.SpecularExponent = 20;
props.SpecularStrength = 1;
props.FaceColor= 'texture';
props.EdgeColor = 'none';
props.FaceLighting = 'phong';
props.Cdata = topo;
alpha_earth = pi/1;
x_new = x*cos(alpha_earth) + y*sin(alpha_earth);
y_new = -x*sin(alpha_earth) + y*cos(alpha_earth);
if hF
figure(hF);
hA = gca;
else
set(handles.fig_main,'CurrentAxes', handles.axes_3D)
hA = handles.axes_3D;
end
cla(hA);
surface(x_new, y_new, z, props);
light('position',[-1 -1 1*tan(Earth_axe_angl)]*R_earth_equa*2);
% campos([+1 0 -1*tan(Earth_axe_angl)]*R_earth_equa*2);
% camtarget([0 0 0])
view(3)
view(40,-8);
hold(hA, 'on');
plot3(hA, Xest.x, Xest.y, Xest.z, Xist.x, Xist.y, Xist.z, 'LineWidth', 2);
hold(hA, 'off');
xlim([-8e6 8e6]); ylim([-8e6 8e6]); zlim([-8e6 8e6]);
title(hA, 'Space View');
if ~hF
set(hA, 'XTick', []);
set(hA, 'YTick', []);
set(hA, 'ZTick', []);
set(hA, 'FontSize', Font_Size);
else
xlabel(hA, 'x, m');
ylabel(hA, 'y, m');
zlabel(hA, 'z, m');
grid(hA, 'on');
end
%> @brief 3D-Earth, SV's orbits
%> @param handles Main handles struct
%> @param hF Flag (handle) for separate window
% ======================================================================
function plot_axes_Earth(handles, hF)
globals;
R_earth_pol = 6356777; % Polar radius of Earth
R_earth_equa = 6378160; % Equatorial radius of Earth
Earth_axe_angl = deg2rad(23);
[x,y,z] = sphere(50);
x = R_earth_equa * x; y = R_earth_equa * y; z = R_earth_pol * z;
load('topo.mat','topo','topomap1');
% cla reset
% axis square off
props.AmbientStrength = 0.1;
props.DiffuseStrength = 1;
props.SpecularColorReflectance = .5;
props.SpecularExponent = 20;
props.SpecularStrength = 1;
props.FaceColor= 'texture';
props.EdgeColor = 'none';
props.FaceLighting = 'phong';
props.Cdata = topo;
alpha_earth = pi/1;
x_new = x*cos(alpha_earth) + y*sin(alpha_earth);
y_new = -x*sin(alpha_earth) + y*cos(alpha_earth);
if hF
figure(hF);
hA = gca;
else
set(handles.fig_main,'CurrentAxes', handles.axes_3D)
hA = handles.axes_3D;
end
cla(hA);
surface(x_new, y_new, z, props);
light('position',[-1 -1 1*tan(Earth_axe_angl)]*R_earth_equa*2);
% campos([+1 0 -1*tan(Earth_axe_angl)]*R_earth_equa*2);
% camtarget([0 0 0])
view(3)
view(40,-8);
hold(hA, 'on');
plot3(hA, Xest.x, Xest.y, Xest.z, Xist.x, Xist.y, Xist.z, 'LineWidth', 2);
hold(hA, 'off');
xlim([-8e6 8e6]); ylim([-8e6 8e6]); zlim([-8e6 8e6]);
title(hA, 'Space View');
if ~hF
set(hA, 'XTick', []);
set(hA, 'YTick', []);
set(hA, 'ZTick', []);
set(hA, 'FontSize', Font_Size);
else
xlabel(hA, 'x, m');
ylabel(hA, 'y, m');
zlabel(hA, 'z, m');
grid(hA, 'on');
end
[править] plot_axes_OE.m
%> ======================================================================
%> @brief plot True trajectory graphs
%> @param handles Main handles struct
%> @param hF Flag (handle) for separate window
%> @param ij Indexies of axes (2-el string)
% ======================================================================
function plot_axes_OE(handles, hF, ij)
globals;
if hF
figure(hF);
hA = gca;
XLab = 't, s';
else
hA = eval(['handles.axes_OE_' ij]);
set(handles.fig_main,'CurrentAxes', hA)
XLab = '';
YLab = '';
end
titl = '';
X1 = tmod; X2 = tmod; X3 = tmod; X4 = tmod;
Y1 = nan(1, Nmod); Y2 = nan(1, Nmod); Y3 = nan(1, Nmod); Y4 = nan(1, Nmod);
leg_str ='';
switch ij
case '11'
Y1 = Xist.sqrtA;
YLab = 'a^{1/2}, m^{1/2}';
titl = 'Square root of semimajor axis';
case '12'
Y1 = Xist.e;
YLab = 'e';
titl = 'Eccentricity';
case '13'
Y1 = Xist.Crc;
Y2 = Xist.Crs;
YLab = 'C_{rc}, C_{rs}, m';
titl = 'Harmonic coefficients for radius';
leg_str = 'legend(hA, ''C_{rc}'', ''C_{rs}'')';
case '14'
Y1 = Xist.r;
Y2 = Xist.A;
YLab = 'r, a, m';
titl = 'Full radius, semimajor axis';
leg_str = 'legend(hA, ''Radius'', ''Semimajor axis'')';
case '21'
Y1 = Xist.i0;
YLab = 'i_0, rad';
titl = 'Base inclination';
case '22'
Y1 = Xist.i_dot;
YLab = 'i_{dot}, rad/s';
titl = 'Derivative of the base inclination';
case '23'
Y1 = Xist.Cic;
Y2 = Xist.Cis;
YLab = 'C_{ic}, C_{is}, rad';
titl = 'Harmonic coefficients for inclination';
leg_str = 'legend(hA, ''C_{ic}'', ''C_{is}'')';
case '24'
Y1 = Xist.i;
YLab = 'i, rad';
titl = 'Full inclination';
case '31'
Y1 = Xist.M0;
Y2 = Xist.E;
Y3 = Xist.theta;
YLab = 'M_0, E, \theta, rad';
titl = 'Mean, eccentricity and true anomaly';
leg_str = 'legend(hA, ''M_0'', ''E'', ''\theta'')';
case '32'
Y1 = Xist.omega;
YLab = '\omega, rad';
titl = 'Argument of periapsis';
case '33'
Y1 = Xist.Cuc;
Y2 = Xist.Cus;
YLab = 'C_{uc}, C_{us}, rad';
titl = 'Harmonic coefficients for argument of latitude';
leg_str = 'legend(hA, ''C_{uc}'', ''C_{us}'')';
case '34'
Y1 = Xist.u;
YLab = 'u, rad';
titl = 'Argument of latitude';
case '41'
Y1 = Xist.Omega;
YLab = '\Omega, rad';
titl = 'Longitude of the ascending node';
case '42'
Y1 = Xist.Omega_dot;
YLab = '\Omega_{dot}, rad/s';
titl = 'Derivative of the longitude of the ascending node';
case '43'
Y1 = Xist.x0;
Y2 = Xist.y0;
Y3 = Xist.z0;
YLab = 'x_0, y_0, z_0, m';
titl = 'Coordinates in the inertial coordinate system';
leg_str = 'legend(hA, ''x_0'', ''y_0'', ''z_0'')';
case '44'
Y1 = Xist.d_x0;
Y2 = Xist.d_y0;
Y3 = Xist.d_z0;
YLab = 'V_x, V_y, V_z, m';
titl = 'Velocities in the inertial coordinate system';
leg_str = 'legend(hA, ''V_x'', ''V_y'', ''V_z'')';
end
if isnan(Y4(1))
if isnan(Y3(1))
if isnan(Y2(1))
if isnan(Y1(1))
set(hA, 'Visible', 'off');
else
plot(hA, X1, Y1);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4);
set(hA, 'Visible', 'on');
end
if hF
title(hA, titl);
eval(leg_str);
end
ly = ylabel(YLab);
lx = xlabel(XLab);
grid(hA, 'on');
if ~hF
set(hA, 'XTick', []);
set(hA, 'YTick', []);
set(ly, 'FontSize', Font_Size);
end
%> @brief plot True trajectory graphs
%> @param handles Main handles struct
%> @param hF Flag (handle) for separate window
%> @param ij Indexies of axes (2-el string)
% ======================================================================
function plot_axes_OE(handles, hF, ij)
globals;
if hF
figure(hF);
hA = gca;
XLab = 't, s';
else
hA = eval(['handles.axes_OE_' ij]);
set(handles.fig_main,'CurrentAxes', hA)
XLab = '';
YLab = '';
end
titl = '';
X1 = tmod; X2 = tmod; X3 = tmod; X4 = tmod;
Y1 = nan(1, Nmod); Y2 = nan(1, Nmod); Y3 = nan(1, Nmod); Y4 = nan(1, Nmod);
leg_str ='';
switch ij
case '11'
Y1 = Xist.sqrtA;
YLab = 'a^{1/2}, m^{1/2}';
titl = 'Square root of semimajor axis';
case '12'
Y1 = Xist.e;
YLab = 'e';
titl = 'Eccentricity';
case '13'
Y1 = Xist.Crc;
Y2 = Xist.Crs;
YLab = 'C_{rc}, C_{rs}, m';
titl = 'Harmonic coefficients for radius';
leg_str = 'legend(hA, ''C_{rc}'', ''C_{rs}'')';
case '14'
Y1 = Xist.r;
Y2 = Xist.A;
YLab = 'r, a, m';
titl = 'Full radius, semimajor axis';
leg_str = 'legend(hA, ''Radius'', ''Semimajor axis'')';
case '21'
Y1 = Xist.i0;
YLab = 'i_0, rad';
titl = 'Base inclination';
case '22'
Y1 = Xist.i_dot;
YLab = 'i_{dot}, rad/s';
titl = 'Derivative of the base inclination';
case '23'
Y1 = Xist.Cic;
Y2 = Xist.Cis;
YLab = 'C_{ic}, C_{is}, rad';
titl = 'Harmonic coefficients for inclination';
leg_str = 'legend(hA, ''C_{ic}'', ''C_{is}'')';
case '24'
Y1 = Xist.i;
YLab = 'i, rad';
titl = 'Full inclination';
case '31'
Y1 = Xist.M0;
Y2 = Xist.E;
Y3 = Xist.theta;
YLab = 'M_0, E, \theta, rad';
titl = 'Mean, eccentricity and true anomaly';
leg_str = 'legend(hA, ''M_0'', ''E'', ''\theta'')';
case '32'
Y1 = Xist.omega;
YLab = '\omega, rad';
titl = 'Argument of periapsis';
case '33'
Y1 = Xist.Cuc;
Y2 = Xist.Cus;
YLab = 'C_{uc}, C_{us}, rad';
titl = 'Harmonic coefficients for argument of latitude';
leg_str = 'legend(hA, ''C_{uc}'', ''C_{us}'')';
case '34'
Y1 = Xist.u;
YLab = 'u, rad';
titl = 'Argument of latitude';
case '41'
Y1 = Xist.Omega;
YLab = '\Omega, rad';
titl = 'Longitude of the ascending node';
case '42'
Y1 = Xist.Omega_dot;
YLab = '\Omega_{dot}, rad/s';
titl = 'Derivative of the longitude of the ascending node';
case '43'
Y1 = Xist.x0;
Y2 = Xist.y0;
Y3 = Xist.z0;
YLab = 'x_0, y_0, z_0, m';
titl = 'Coordinates in the inertial coordinate system';
leg_str = 'legend(hA, ''x_0'', ''y_0'', ''z_0'')';
case '44'
Y1 = Xist.d_x0;
Y2 = Xist.d_y0;
Y3 = Xist.d_z0;
YLab = 'V_x, V_y, V_z, m';
titl = 'Velocities in the inertial coordinate system';
leg_str = 'legend(hA, ''V_x'', ''V_y'', ''V_z'')';
end
if isnan(Y4(1))
if isnan(Y3(1))
if isnan(Y2(1))
if isnan(Y1(1))
set(hA, 'Visible', 'off');
else
plot(hA, X1, Y1);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4);
set(hA, 'Visible', 'on');
end
if hF
title(hA, titl);
eval(leg_str);
end
ly = ylabel(YLab);
lx = xlabel(XLab);
grid(hA, 'on');
if ~hF
set(hA, 'XTick', []);
set(hA, 'YTick', []);
set(ly, 'FontSize', Font_Size);
end
[править] plot_axes_Track.m
%> ======================================================================
%> @brief plot Tracking graphs
%> @param handles Main handles struct
%> @param hF Flag (handle) for separate window
%> @param ij Indexies of axes (2-el string)
% ======================================================================
function plot_axes_Track(handles, hF, ij)
globals;
if hF
figure(hF);
hA = gca;
XLab = 't, s';
else
hA = eval(['handles.axes_Track_' ij]);
set(handles.fig_main,'CurrentAxes', hA)
XLab = '';
YLab = '';
end
titl = '';
X1 = tmod; X2 = tmod; X3 = tmod; X4 = tmod; X5 = tmod; X6 = tmod;
Y1 = nan(1, Nmod); Y2 = nan(1, Nmod); Y3 = nan(1, Nmod); Y4 = nan(1, Nmod); Y5 = nan(1, Nmod); Y6 = nan(1, Nmod);
switch ij
case '11'
Y1 = Xest.e;
Y2 = Xest_won.e;
YLab = 'e';
titl = 'Eccentricity';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '12'
Y1 = Xest.d_e;
Y2 = Xest_won.d_e;
YLab = 'e''';
titl = 'Eccentricity rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '13'
Y1 = Xest.p;
Y2 = Xest_won.p;
YLab = 'p, m';
titl = 'Focal parameter';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '14'
Y1 = Xest.d_p;
Y2 = Xest_won.d_p;
YLab = 'p'', m/s';
titl = 'Focal parameter rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '21'
Y1 = Xest.theta;
Y2 = Xest_won.theta;
YLab = '\theta, rad';
titl = 'True anomaly';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '22'
Y1 = Xest.d_theta;
Y2 = Xest_won.d_theta;
YLab = '\theta'', rad/s';
titl = 'True anomaly rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '23'
Y1 = Xest.omega;
Y2 = Xest_won.omega;
YLab = '\omega, rad';
titl = 'Argument of periapsis';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '24'
Y1 = Xest.d_omega;
Y2 = Xest_won.d_omega;
YLab = '\omega'', rad/s';
titl = 'Argument of periapsis rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '31'
Y1 = Xest.Omega;
Y2 = Xest_won.Omega;
YLab = '\Omega, rad';
titl = 'Longitude of the ascending node';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '32'
Y1 = Xest.d_Omega;
Y2 = Xest_won.d_Omega;
YLab = '\Omega'', rad/s';
titl = 'Longitude of the ascending node rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '33'
Y1 = Xest.i;
Y2 = Xest_won.i;
YLab = 'i, rad';
titl = 'Inclination';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '34'
Y1 = Xest.d_i;
Y2 = Xest_won.d_i;
YLab = 'i'', rad/s';
titl = 'Inclination rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '41'
Y1 = Xest.x0;
Y2 = Xest.y0;
Y3 = Xest.z0;
Y4 = Xest_won.x0;
Y5 = Xest_won.y0;
Y6 = Xest_won.z0;
YLab = 'x_0, y_0, z_0, m';
titl = 'Coordinates in the inertial coordinate system';
leg_str = 'legend(hA, ''Estimation x_0'', ''Estimation y_0'', ''Estimation z_0'', ''True x_0'', ''True y_0'', ''True z_0'')';
case '42'
Y1 = Xest.d_x0;
Y2 = Xest.d_y0;
Y3 = Xest.d_z0;
Y4 = Xest_won.d_x0;
Y5 = Xest_won.d_y0;
Y6 = Xest_won.d_z0;
YLab = 'V_x, V_y, V_z, m/s';
titl = 'Velocities in the inertial coordinate system';
leg_str = 'legend(hA, ''Estimation V_x'', ''Estimation V_y'', ''Estimation V_z'', ''True V_x'', ''True V_y'', ''True V_z'')';
case '43'
Y1 = sqrt((Xest.x0 - Xist.x0).^2 + (Xest.y0 - Xist.y0).^2 + (Xest.z0 - Xist.z0).^2);
Y2 = sqrt((Xest_won.x0 - Xist.x0).^2 + (Xest_won.y0 - Xist.y0).^2 + (Xest_won.z0 - Xist.z0).^2);
YLab = 'Error XYZ, m';
titl = 'Error of coordinate estimation';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '44'
Y1 = sqrt((Xest.d_x0 - Xist.d_x0).^2 + (Xest.d_y0 - Xist.d_y0).^2 + (Xest.d_z0 - Xist.d_z0).^2);
Y2 = sqrt((Xest_won.d_x0 - Xist.d_x0).^2 + (Xest_won.d_y0 - Xist.d_y0).^2 + (Xest_won.d_z0 - Xist.d_z0).^2);
YLab = 'Error V, m/s';
titl = 'Error of velocity estimation';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
end
if isnan(Y6(1))
if isnan(Y5(1))
if isnan(Y4(1))
if isnan(Y3(1))
if isnan(Y2(1))
if isnan(Y1(1))
set(hA, 'Visible', 'off');
else
plot(hA, X1, Y1);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4, X5, Y5);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4, X5, Y5, X6, Y6);
set(hA, 'Visible', 'on');
end
if hF
title(hA, titl);
eval(leg_str);
end
ly = ylabel(YLab);
lx = xlabel(XLab);
grid(hA, 'on');
if ~hF
set(hA, 'XTick', []);
set(hA, 'YTick', []);
set(ly, 'FontSize', Font_Size);
end
%> @brief plot Tracking graphs
%> @param handles Main handles struct
%> @param hF Flag (handle) for separate window
%> @param ij Indexies of axes (2-el string)
% ======================================================================
function plot_axes_Track(handles, hF, ij)
globals;
if hF
figure(hF);
hA = gca;
XLab = 't, s';
else
hA = eval(['handles.axes_Track_' ij]);
set(handles.fig_main,'CurrentAxes', hA)
XLab = '';
YLab = '';
end
titl = '';
X1 = tmod; X2 = tmod; X3 = tmod; X4 = tmod; X5 = tmod; X6 = tmod;
Y1 = nan(1, Nmod); Y2 = nan(1, Nmod); Y3 = nan(1, Nmod); Y4 = nan(1, Nmod); Y5 = nan(1, Nmod); Y6 = nan(1, Nmod);
switch ij
case '11'
Y1 = Xest.e;
Y2 = Xest_won.e;
YLab = 'e';
titl = 'Eccentricity';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '12'
Y1 = Xest.d_e;
Y2 = Xest_won.d_e;
YLab = 'e''';
titl = 'Eccentricity rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '13'
Y1 = Xest.p;
Y2 = Xest_won.p;
YLab = 'p, m';
titl = 'Focal parameter';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '14'
Y1 = Xest.d_p;
Y2 = Xest_won.d_p;
YLab = 'p'', m/s';
titl = 'Focal parameter rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '21'
Y1 = Xest.theta;
Y2 = Xest_won.theta;
YLab = '\theta, rad';
titl = 'True anomaly';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '22'
Y1 = Xest.d_theta;
Y2 = Xest_won.d_theta;
YLab = '\theta'', rad/s';
titl = 'True anomaly rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '23'
Y1 = Xest.omega;
Y2 = Xest_won.omega;
YLab = '\omega, rad';
titl = 'Argument of periapsis';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '24'
Y1 = Xest.d_omega;
Y2 = Xest_won.d_omega;
YLab = '\omega'', rad/s';
titl = 'Argument of periapsis rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '31'
Y1 = Xest.Omega;
Y2 = Xest_won.Omega;
YLab = '\Omega, rad';
titl = 'Longitude of the ascending node';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '32'
Y1 = Xest.d_Omega;
Y2 = Xest_won.d_Omega;
YLab = '\Omega'', rad/s';
titl = 'Longitude of the ascending node rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '33'
Y1 = Xest.i;
Y2 = Xest_won.i;
YLab = 'i, rad';
titl = 'Inclination';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '34'
Y1 = Xest.d_i;
Y2 = Xest_won.d_i;
YLab = 'i'', rad/s';
titl = 'Inclination rate';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '41'
Y1 = Xest.x0;
Y2 = Xest.y0;
Y3 = Xest.z0;
Y4 = Xest_won.x0;
Y5 = Xest_won.y0;
Y6 = Xest_won.z0;
YLab = 'x_0, y_0, z_0, m';
titl = 'Coordinates in the inertial coordinate system';
leg_str = 'legend(hA, ''Estimation x_0'', ''Estimation y_0'', ''Estimation z_0'', ''True x_0'', ''True y_0'', ''True z_0'')';
case '42'
Y1 = Xest.d_x0;
Y2 = Xest.d_y0;
Y3 = Xest.d_z0;
Y4 = Xest_won.d_x0;
Y5 = Xest_won.d_y0;
Y6 = Xest_won.d_z0;
YLab = 'V_x, V_y, V_z, m/s';
titl = 'Velocities in the inertial coordinate system';
leg_str = 'legend(hA, ''Estimation V_x'', ''Estimation V_y'', ''Estimation V_z'', ''True V_x'', ''True V_y'', ''True V_z'')';
case '43'
Y1 = sqrt((Xest.x0 - Xist.x0).^2 + (Xest.y0 - Xist.y0).^2 + (Xest.z0 - Xist.z0).^2);
Y2 = sqrt((Xest_won.x0 - Xist.x0).^2 + (Xest_won.y0 - Xist.y0).^2 + (Xest_won.z0 - Xist.z0).^2);
YLab = 'Error XYZ, m';
titl = 'Error of coordinate estimation';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
case '44'
Y1 = sqrt((Xest.d_x0 - Xist.d_x0).^2 + (Xest.d_y0 - Xist.d_y0).^2 + (Xest.d_z0 - Xist.d_z0).^2);
Y2 = sqrt((Xest_won.d_x0 - Xist.d_x0).^2 + (Xest_won.d_y0 - Xist.d_y0).^2 + (Xest_won.d_z0 - Xist.d_z0).^2);
YLab = 'Error V, m/s';
titl = 'Error of velocity estimation';
leg_str = 'legend(hA, ''Estimation'', ''True'')';
end
if isnan(Y6(1))
if isnan(Y5(1))
if isnan(Y4(1))
if isnan(Y3(1))
if isnan(Y2(1))
if isnan(Y1(1))
set(hA, 'Visible', 'off');
else
plot(hA, X1, Y1);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4, X5, Y5);
set(hA, 'Visible', 'on');
end
else
plot(hA, X1, Y1, X2, Y2, X3, Y3, X4, Y4, X5, Y5, X6, Y6);
set(hA, 'Visible', 'on');
end
if hF
title(hA, titl);
eval(leg_str);
end
ly = ylabel(YLab);
lx = xlabel(XLab);
grid(hA, 'on');
if ~hF
set(hA, 'XTick', []);
set(hA, 'YTick', []);
set(ly, 'FontSize', Font_Size);
end
[править] fsolve_Kepler.m
function Ysolve = fsolve_Kepler(Xs, Xizm, Yizm, Zizm, VXizm, VYizm, VZizm)
global mu_earth
%%%%Xsolve = r, u, Omega, i, d_r, d_u
% Xsolve = theta, omega_p, Omega, i, e, p
theta = Xs(1);
omega_p = Xs(2);
Omega = Xs(3);
i = Xs(4);
e = Xs(5);
p = Xs(6);
[x, y, z, Vx, Vy, Vz] = get_vector_XV( e, p, theta, omega_p, Omega, i);
ErrX = Xizm - x;
ErrY = Yizm - y;
ErrZ = Zizm - z;
ErrVx = VXizm - Vx;
ErrVy = VYizm - Vy;
ErrVz = VZizm - Vz;
Ysolve = [ErrX, ErrY, ErrZ, ErrVx, ErrVy, ErrVz];
end
global mu_earth
%%%%Xsolve = r, u, Omega, i, d_r, d_u
% Xsolve = theta, omega_p, Omega, i, e, p
theta = Xs(1);
omega_p = Xs(2);
Omega = Xs(3);
i = Xs(4);
e = Xs(5);
p = Xs(6);
[x, y, z, Vx, Vy, Vz] = get_vector_XV( e, p, theta, omega_p, Omega, i);
ErrX = Xizm - x;
ErrY = Yizm - y;
ErrZ = Zizm - z;
ErrVx = VXizm - Vx;
ErrVy = VYizm - Vy;
ErrVz = VZizm - Vz;
Ysolve = [ErrX, ErrY, ErrZ, ErrVx, ErrVy, ErrVz];
end
[править] solve_wo_noise.m
%> ======================================================================
%> @brief Calculate true osculating elements for true trajectory
%> @param handles Main handles struct
%> ======================================================================
function solve_wo_noise( handles )
globals;
% Initial point
Xs(1) = Xist.theta(1);
Xs(2) = Xist.omega(1);
Xs(3) = Xist.Omega(1);
Xs(4) = Xist.i(1);
Xs(5) = Xist.e(1);
Xs(6) = Xist.p(1);
set(handles.pb_Solve, 'Enable', 'off');
drawnow
pause(0.01);
for i = 1:Nmod
% Calculating osculating elements by means of the true coordinates and velocities
Xs = fsolve(@(Xfs)(fsolve_Kepler(Xfs, Xist.x0(i), Xist.y0(i), Xist.z0(i),...
Xist.d_x0(i), Xist.d_y0(i), Xist.d_z0(i))), Xs, options_solve);
% Saving of results
Xest_won.e(i) = Xs(5);
Xest_won.theta(i) = Xs(1);
Xest_won.omega(i) = Xs(2);
Xest_won.Omega(i) = Xs(3);
Xest_won.i(i) = Xs(4);
Xest_won.p(i) = Xs(6);
Xest_won.r(i) = Xest_won.p(i) ./ (1 + Xest_won.e(i)*cos(Xest_won.theta(i)));
% Get coordinates for oculating elements (for test)
[Xest_won.x0(i) Xest_won.y0(i) Xest_won.z0(i) ...
Xest_won.d_x0(i) Xest_won.d_y0(i) Xest_won.d_z0(i)] = ...
get_vector_XV( Xest_won.e(i), Xest_won.p(i), Xest_won.theta(i), ...
Xest_won.omega(i), Xest_won.Omega(i), Xest_won.i(i));
% [Xest_won.x0(i) Xest_won.y0(i) Xest_won.z0(i)] = get_vector_XYZ( Xest_won.r(i),...
% Xest_won.Omega(i), Xest_won.i(i), Xest_won.theta(i)+Xest_won.omega(i) );
if ~mod(i, fix(Nmod/100))
set(handles.txt_Solve, 'String', sprintf('%.0f %%', i/Nmod*100));
end
drawnow
pause(0.01);
end
set(handles.txt_Solve, 'String', sprintf('%.0f %%', 100));
% Calculation of derivatives
Xest_won.d_theta = diff(Xest_won.theta)/dTmod;
Xest_won.d_theta(end+1)=Xest_won.d_theta(end);
Xest_won.d_omega = diff(Xest_won.omega)/dTmod;
Xest_won.d_omega(end+1)=Xest_won.d_omega(end);
Xest_won.d_Omega = diff(Xest_won.Omega)/dTmod;
Xest_won.d_Omega(end+1)=Xest_won.d_Omega(end);
Xest_won.d_i = diff(Xest_won.i)/dTmod;
Xest_won.d_i(end+1)=Xest_won.d_i(end);
Xest_won.d_e = diff(Xest_won.e)/dTmod;
Xest_won.d_e(end+1)=Xest_won.d_e(end);
Xest_won.d_p = diff(Xest_won.p)/dTmod;
Xest_won.d_p(end+1)=Xest_won.d_p(end);
Xest_won.d_r = diff(Xest_won.r)/dTmod;
Xest_won.d_r(end+1)=Xest_won.d_r(end);
for i = 1:Nmod
% Saving of results
if Xest_won.e(i) > 0;
Xest_won.e(i) = Xest_won.e(i);
Xest_won.theta(i) = mod_pm_pi(Xest_won.theta(i));
Xest_won.omega(i) = mod_pm_pi(Xest_won.omega(i));
else
Xest_won.e(i) = -Xest_won.e(i);
Xest_won.theta(i) = mod_pm_pi(-Xest_won.theta(i));
Xest_won.omega(i) = mod_pm_pi(-Xest_won.omega(i));
end
Xest_won.Omega(i) = mod_pm_pi(Xest_won.Omega(i));
Xest_won.i(i) = mod_pm_pi(Xest_won.i(i));
end
% Output results to form
for i = 1:5
for j = 1:5
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'DrawMode', 'fast');
plot_axes_Track(handles, 0, [num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Tag', ['axes_Track_' num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_Track_ButtonDownFcn'',hObject,eventdata,guidata(hObject))'));
end
end
set(handles.pan_Orbital_Elements, 'Visible', 'off');
set(handles.pan_Tracking, 'Visible', 'on');
set(handles.pb_Track, 'Enable', 'on');
set(handles.ed_stdX, 'Enable', 'on');
set(handles.ed_stdV, 'Enable', 'on');
end
%> @brief Calculate true osculating elements for true trajectory
%> @param handles Main handles struct
%> ======================================================================
function solve_wo_noise( handles )
globals;
% Initial point
Xs(1) = Xist.theta(1);
Xs(2) = Xist.omega(1);
Xs(3) = Xist.Omega(1);
Xs(4) = Xist.i(1);
Xs(5) = Xist.e(1);
Xs(6) = Xist.p(1);
set(handles.pb_Solve, 'Enable', 'off');
drawnow
pause(0.01);
for i = 1:Nmod
% Calculating osculating elements by means of the true coordinates and velocities
Xs = fsolve(@(Xfs)(fsolve_Kepler(Xfs, Xist.x0(i), Xist.y0(i), Xist.z0(i),...
Xist.d_x0(i), Xist.d_y0(i), Xist.d_z0(i))), Xs, options_solve);
% Saving of results
Xest_won.e(i) = Xs(5);
Xest_won.theta(i) = Xs(1);
Xest_won.omega(i) = Xs(2);
Xest_won.Omega(i) = Xs(3);
Xest_won.i(i) = Xs(4);
Xest_won.p(i) = Xs(6);
Xest_won.r(i) = Xest_won.p(i) ./ (1 + Xest_won.e(i)*cos(Xest_won.theta(i)));
% Get coordinates for oculating elements (for test)
[Xest_won.x0(i) Xest_won.y0(i) Xest_won.z0(i) ...
Xest_won.d_x0(i) Xest_won.d_y0(i) Xest_won.d_z0(i)] = ...
get_vector_XV( Xest_won.e(i), Xest_won.p(i), Xest_won.theta(i), ...
Xest_won.omega(i), Xest_won.Omega(i), Xest_won.i(i));
% [Xest_won.x0(i) Xest_won.y0(i) Xest_won.z0(i)] = get_vector_XYZ( Xest_won.r(i),...
% Xest_won.Omega(i), Xest_won.i(i), Xest_won.theta(i)+Xest_won.omega(i) );
if ~mod(i, fix(Nmod/100))
set(handles.txt_Solve, 'String', sprintf('%.0f %%', i/Nmod*100));
end
drawnow
pause(0.01);
end
set(handles.txt_Solve, 'String', sprintf('%.0f %%', 100));
% Calculation of derivatives
Xest_won.d_theta = diff(Xest_won.theta)/dTmod;
Xest_won.d_theta(end+1)=Xest_won.d_theta(end);
Xest_won.d_omega = diff(Xest_won.omega)/dTmod;
Xest_won.d_omega(end+1)=Xest_won.d_omega(end);
Xest_won.d_Omega = diff(Xest_won.Omega)/dTmod;
Xest_won.d_Omega(end+1)=Xest_won.d_Omega(end);
Xest_won.d_i = diff(Xest_won.i)/dTmod;
Xest_won.d_i(end+1)=Xest_won.d_i(end);
Xest_won.d_e = diff(Xest_won.e)/dTmod;
Xest_won.d_e(end+1)=Xest_won.d_e(end);
Xest_won.d_p = diff(Xest_won.p)/dTmod;
Xest_won.d_p(end+1)=Xest_won.d_p(end);
Xest_won.d_r = diff(Xest_won.r)/dTmod;
Xest_won.d_r(end+1)=Xest_won.d_r(end);
for i = 1:Nmod
% Saving of results
if Xest_won.e(i) > 0;
Xest_won.e(i) = Xest_won.e(i);
Xest_won.theta(i) = mod_pm_pi(Xest_won.theta(i));
Xest_won.omega(i) = mod_pm_pi(Xest_won.omega(i));
else
Xest_won.e(i) = -Xest_won.e(i);
Xest_won.theta(i) = mod_pm_pi(-Xest_won.theta(i));
Xest_won.omega(i) = mod_pm_pi(-Xest_won.omega(i));
end
Xest_won.Omega(i) = mod_pm_pi(Xest_won.Omega(i));
Xest_won.i(i) = mod_pm_pi(Xest_won.i(i));
end
% Output results to form
for i = 1:5
for j = 1:5
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'DrawMode', 'fast');
plot_axes_Track(handles, 0, [num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Tag', ['axes_Track_' num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_Track_ButtonDownFcn'',hObject,eventdata,guidata(hObject))'));
end
end
set(handles.pan_Orbital_Elements, 'Visible', 'off');
set(handles.pan_Tracking, 'Visible', 'on');
set(handles.pb_Track, 'Enable', 'on');
set(handles.ed_stdX, 'Enable', 'on');
set(handles.ed_stdV, 'Enable', 'on');
end
[править] get_vector_XV.m
function [x, y, z, Vx, Vy, Vz] = get_vector_XV( e, p, theta, omega, Omega, i)
%GET_VECTOR_XV Get vectors x, y, z and Vx, Vy, Vz
global mu_earth
munapi = sqrt(mu_earth / p);
Vr = munapi*e*sin(theta);
Vu = munapi*(1+e*cos(theta));
r = p / (1+e*cos(theta));
u = theta + omega;
xyz = U3(-Omega)*U1(-i)*U3(-u)*[r; 0; 0];
x = xyz(1);
y = xyz(2);
z = xyz(3);
Vx = Vr.*x./r - Vu.*(sin(u).*cos(Omega) + cos(u).*sin(Omega).*cos(i));
Vy = Vr.*y./r - Vu.*(sin(u).*sin(Omega) - cos(u).*cos(Omega).*cos(i));
Vz = Vr.*z./r + Vu.*cos(u).*sin(i);
end
%GET_VECTOR_XV Get vectors x, y, z and Vx, Vy, Vz
global mu_earth
munapi = sqrt(mu_earth / p);
Vr = munapi*e*sin(theta);
Vu = munapi*(1+e*cos(theta));
r = p / (1+e*cos(theta));
u = theta + omega;
xyz = U3(-Omega)*U1(-i)*U3(-u)*[r; 0; 0];
x = xyz(1);
y = xyz(2);
z = xyz(3);
Vx = Vr.*x./r - Vu.*(sin(u).*cos(Omega) + cos(u).*sin(Omega).*cos(i));
Vy = Vr.*y./r - Vu.*(sin(u).*sin(Omega) - cos(u).*cos(Omega).*cos(i));
Vz = Vr.*z./r + Vu.*cos(u).*sin(i);
end
[править] mod_pm_pi.m
function y = mod_pm_pi( x )
%MOD_PM_PI mod [-pi; pi];
y = mod(x + pi, 2*pi) - pi;
end
%MOD_PM_PI mod [-pi; pi];
y = mod(x + pi, 2*pi) - pi;
end
[править] resize_arrays.m
tmod = tmod(1:Nmod);
%tput;
% --- Executes on mouse press over axes background.
function axes_3D_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_3D (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
globals;
plot_axes_Earth(handles, next_hF());
% --- Executes on mouse press over axes background.
function axes_OE_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_OE_ (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
dig = get(hObject, 'Tag'); dig = dig(end-1:end);
plot_axes_OE(handles, next_hF(), dig)
% --- Executes on mouse press over axes background.
function axes_Track_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_OE_ (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
dig = get(hObject, 'Tag'); dig = dig(end-1:end);
plot_axes_Track(handles, next_hF(), dig)
% --- Executes on button press in pb_Track.
function pb_Track_Callback(hObject, eventdata, handles)
% hObject handle to pb_Track (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
std_x = str2double(get(handles.ed_stdX, 'String'));
if ~((std_x >= 0) True vector
Xist.dn = Xist.dn(1:Nmod);
Xist.M0 = Xist.M0(1:Nmod);
Xist.e = Xist.e(1:Nmod);
Xist.d_e = Xist.d_e(1:Nmod);
Xist.dd_e = Xist.dd_e(1:Nmod);
Xist.A = Xist.A(1:Nmod);
Xist.d_A = Xist.d_A(1:Nmod);
Xist.dd_A = Xist.dd_A(1:Nmod);
Xist.sqrtA = Xist.sqrtA(1:Nmod);
Xist.Omega = Xist.Omega(1:Nmod);
Xist.d_Omega = Xist.d_Omega(1:Nmod);
Xist.dd_Omega = Xist.dd_Omega(1:Nmod);
Xist.lambda = Xist.lambda(1:Nmod);
Xist.d_lambda = Xist.d_lambda(1:Nmod);
Xist.dd_lambda = Xist.dd_lambda(1:Nmod);
Xist.Omega_dot = Xist.Omega_dot(1:Nmod);
Xist.Cis = Xist.Cis(1:Nmod);
Xist.Cic = Xist.Cic(1:Nmod);
Xist.Crs = Xist.Crs(1:Nmod);
Xist.Crc = Xist.Crc(1:Nmod);
Xist.Cus = Xist.Cus(1:Nmod);
Xist.Cuc = Xist.Cuc(1:Nmod);
Xist.omega = Xist.omega(1:Nmod);
Xist.d_omega = Xist.d_omega(1:Nmod);
Xist.dd_omega = Xist.dd_omega(1:Nmod);
Xist.i0 = Xist.i0(1:Nmod);
Xist.i_dot = Xist.i_dot(1:Nmod);
Xist.i = Xist.i(1:Nmod);
Xist.d_i = Xist.d_i(1:Nmod);
Xist.dd_i = Xist.dd_i(1:Nmod);
Xist.u = Xist.u(1:Nmod);
Xist.d_u = Xist.d_u(1:Nmod);
Xist.dd_u = Xist.dd_u(1:Nmod);
Xist.r = Xist.r(1:Nmod);
Xist.d_r = Xist.d_r(1:Nmod);
Xist.dd_r = Xist.dd_r(1:Nmod);
Xist.E = Xist.E(1:Nmod);
Xist.x0 = Xist.x0(1:Nmod);
Xist.y0 = Xist.y0(1:Nmod);
Xist.z0 = Xist.z0(1:Nmod);
Xist.theta1 = Xist.theta1(1:Nmod);
Xist.theta = Xist.theta(1:Nmod);
Xist.d_theta = Xist.d_theta(1:Nmod);
Xist.dd_theta = Xist.dd_theta(1:Nmod);
Xist.p = Xist.p(1:Nmod);
Xist.d_p = Xist.d_p(1:Nmod);
Xist.dd_p = Xist.dd_p(1:Nmod);
Xist.d_x0 = Xist.d_x0(1:Nmod);
Xist.d_y0 = Xist.d_y0(1:Nmod);
Xist.d_z0 = Xist.d_z0(1:Nmod);
Xist.x = Xist.x(1:Nmod);
Xist.y = Xist.y(1:Nmod);
Xist.z = Xist.z(1:Nmod);
% Estimations and extrapolations of Kalman
Xest.e = nan(1, Nmod);
Xest.d_e = nan(1, Nmod);
Xest.p = nan(1, Nmod);
Xest.d_p = nan(1, Nmod);
Xest.theta = nan(1, Nmod);
Xest.d_theta = nan(1, Nmod);
Xest.omega = nan(1, Nmod);
Xest.d_omega = nan(1, Nmod);
Xest.Omega = nan(1, Nmod);
Xest.d_Omega = nan(1, Nmod);
Xest.i = nan(1, Nmod);
Xest.d_i = nan(1, Nmod);
Xest.x0 = nan(1, Nmod);
Xest.y0 = nan(1, Nmod);
Xest.z0 = nan(1, Nmod);
Xest.d_x0 = nan(1, Nmod);
Xest.d_y0 = nan(1, Nmod);
Xest.d_z0 = nan(1, Nmod);
Xest.lambda = nan(1, Nmod);
Xest.x = nan(1, Nmod);
Xest.y = nan(1, Nmod);
Xest.z = nan(1, Nmod);
Xest.X = nan(12,1);
Xextr = Xest; % - extrapolation
% Without noise solution
Xest_won = Xest;
%tput;
% --- Executes on mouse press over axes background.
function axes_3D_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_3D (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
globals;
plot_axes_Earth(handles, next_hF());
% --- Executes on mouse press over axes background.
function axes_OE_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_OE_ (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
dig = get(hObject, 'Tag'); dig = dig(end-1:end);
plot_axes_OE(handles, next_hF(), dig)
% --- Executes on mouse press over axes background.
function axes_Track_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to axes_OE_ (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
dig = get(hObject, 'Tag'); dig = dig(end-1:end);
plot_axes_Track(handles, next_hF(), dig)
% --- Executes on button press in pb_Track.
function pb_Track_Callback(hObject, eventdata, handles)
% hObject handle to pb_Track (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
std_x = str2double(get(handles.ed_stdX, 'String'));
if ~((std_x >= 0) True vector
Xist.dn = Xist.dn(1:Nmod);
Xist.M0 = Xist.M0(1:Nmod);
Xist.e = Xist.e(1:Nmod);
Xist.d_e = Xist.d_e(1:Nmod);
Xist.dd_e = Xist.dd_e(1:Nmod);
Xist.A = Xist.A(1:Nmod);
Xist.d_A = Xist.d_A(1:Nmod);
Xist.dd_A = Xist.dd_A(1:Nmod);
Xist.sqrtA = Xist.sqrtA(1:Nmod);
Xist.Omega = Xist.Omega(1:Nmod);
Xist.d_Omega = Xist.d_Omega(1:Nmod);
Xist.dd_Omega = Xist.dd_Omega(1:Nmod);
Xist.lambda = Xist.lambda(1:Nmod);
Xist.d_lambda = Xist.d_lambda(1:Nmod);
Xist.dd_lambda = Xist.dd_lambda(1:Nmod);
Xist.Omega_dot = Xist.Omega_dot(1:Nmod);
Xist.Cis = Xist.Cis(1:Nmod);
Xist.Cic = Xist.Cic(1:Nmod);
Xist.Crs = Xist.Crs(1:Nmod);
Xist.Crc = Xist.Crc(1:Nmod);
Xist.Cus = Xist.Cus(1:Nmod);
Xist.Cuc = Xist.Cuc(1:Nmod);
Xist.omega = Xist.omega(1:Nmod);
Xist.d_omega = Xist.d_omega(1:Nmod);
Xist.dd_omega = Xist.dd_omega(1:Nmod);
Xist.i0 = Xist.i0(1:Nmod);
Xist.i_dot = Xist.i_dot(1:Nmod);
Xist.i = Xist.i(1:Nmod);
Xist.d_i = Xist.d_i(1:Nmod);
Xist.dd_i = Xist.dd_i(1:Nmod);
Xist.u = Xist.u(1:Nmod);
Xist.d_u = Xist.d_u(1:Nmod);
Xist.dd_u = Xist.dd_u(1:Nmod);
Xist.r = Xist.r(1:Nmod);
Xist.d_r = Xist.d_r(1:Nmod);
Xist.dd_r = Xist.dd_r(1:Nmod);
Xist.E = Xist.E(1:Nmod);
Xist.x0 = Xist.x0(1:Nmod);
Xist.y0 = Xist.y0(1:Nmod);
Xist.z0 = Xist.z0(1:Nmod);
Xist.theta1 = Xist.theta1(1:Nmod);
Xist.theta = Xist.theta(1:Nmod);
Xist.d_theta = Xist.d_theta(1:Nmod);
Xist.dd_theta = Xist.dd_theta(1:Nmod);
Xist.p = Xist.p(1:Nmod);
Xist.d_p = Xist.d_p(1:Nmod);
Xist.dd_p = Xist.dd_p(1:Nmod);
Xist.d_x0 = Xist.d_x0(1:Nmod);
Xist.d_y0 = Xist.d_y0(1:Nmod);
Xist.d_z0 = Xist.d_z0(1:Nmod);
Xist.x = Xist.x(1:Nmod);
Xist.y = Xist.y(1:Nmod);
Xist.z = Xist.z(1:Nmod);
% Estimations and extrapolations of Kalman
Xest.e = nan(1, Nmod);
Xest.d_e = nan(1, Nmod);
Xest.p = nan(1, Nmod);
Xest.d_p = nan(1, Nmod);
Xest.theta = nan(1, Nmod);
Xest.d_theta = nan(1, Nmod);
Xest.omega = nan(1, Nmod);
Xest.d_omega = nan(1, Nmod);
Xest.Omega = nan(1, Nmod);
Xest.d_Omega = nan(1, Nmod);
Xest.i = nan(1, Nmod);
Xest.d_i = nan(1, Nmod);
Xest.x0 = nan(1, Nmod);
Xest.y0 = nan(1, Nmod);
Xest.z0 = nan(1, Nmod);
Xest.d_x0 = nan(1, Nmod);
Xest.d_y0 = nan(1, Nmod);
Xest.d_z0 = nan(1, Nmod);
Xest.lambda = nan(1, Nmod);
Xest.x = nan(1, Nmod);
Xest.y = nan(1, Nmod);
Xest.z = nan(1, Nmod);
Xest.X = nan(12,1);
Xextr = Xest; % - extrapolation
% Without noise solution
Xest_won = Xest;
[править] track_with_noise.m
%> ======================================================================
%> @brief Tracking alghorithm
%> @param handles Main handles struct
%> ======================================================================
function track_with_noise( handles, std_x, std_V )
set(handles.pb_Track, 'Enable', 'off');
pause(0.01);
globals;
% Filter step
T = dTmod;
% Evolutionary matrix
F = [1 T 0 0 0 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0 0 0 0 0;
0 0 1 T 0 0 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0;
0 0 0 0 1 T 0 0 0 0 0 0;
0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 0 0 0 1 T 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 1 T 0 0;
0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 0 0 0 1 T;
0 0 0 0 0 0 0 0 0 0 0 1];
p_mult = 5e7; % To simplify matrix calculations - reducing the dynamic range
% Xextr.X = [Xest_won.e(1); Xest_won.p(1)/p_mult; Xest_won.theta(1); 0; Xest_won.omega(1); 0; Xest_won.Omega(1); 0; Xest_won.i(1); 0];
% Xextr.X = [0.01; 0; Xest_won.p(1)/p_mult; 0; 1; Xest_won.d_theta(1)*1.1; 0; 0; 0; Xest_won.d_Omega(1)*0.9; Xest_won.i(1)*0.9; Xest_won.d_i(1)];
Xextr.X = [0.01; 0; 7e6/p_mult; 0; 1; Xest_won.d_theta(1)*1.1; 0; 0; 0; Xest_won.d_Omega(1)*0.9; Xest_won.i(1)*0.9; Xest_won.d_i(1)];
Xest.X = Xextr.X;
% RMS of shaping noises
std_e = 5e-6 / 15*dTmod;
std_p = 1 / 15*dTmod / p_mult; % [m]
std_theta = 1e-5 / 15*dTmod; % [rad]
std_omega = 1e-6 / 15*dTmod; % [rad]
std_Omega = 1e-8 / 15*dTmod; % [rad]
std_i = 1e-8 / 15*dTmod; % [rad]
Dest = [std_e^2*1e1 0 0 0 0 0 0 0 0 0 0 0
0 std_e^2*1e2 0 0 0 0 0 0 0 0 0 0
0 0 std_p^2*1e3 0 0 0 0 0 0 0 0 0
0 0 0 std_p^2*1e4 0 0 0 0 0 0 0 0
0 0 0 0 std_theta^2*1e2 0 0 0 0 0 0 0
0 0 0 0 0 std_theta^2*1e0 0 0 0 0 0 0
0 0 0 0 0 0 std_omega^2*1e2 0 0 0 0 0
0 0 0 0 0 0 0 std_omega^2*1e2 0 0 0 0
0 0 0 0 0 0 0 0 std_Omega^2*1e2 0 0 0
0 0 0 0 0 0 0 0 0 std_Omega^2*1e2 0 0
0 0 0 0 0 0 0 0 0 0 std_i^2*1e2 0
0 0 0 0 0 0 0 0 0 0 0 std_i^2*1e2];
G = [0 0 0 0 0 0;
1 0 0 0 0 0;
0 0 0 0 0 0;
0 1 0 0 0 0;
0 0 0 0 0 0;
0 0 1 0 0 0;
0 0 0 0 0 0;
0 0 0 1 0 0;
0 0 0 0 0 0;
0 0 0 0 1 0;
0 0 0 0 0 0;
0 0 0 0 0 1];
Dg = [std_e^2 0 0 0 0 0
0 std_p^2 0 0 0 0
0 0 std_theta^2 0 0 0
0 0 0 std_omega^2 0 0
0 0 0 0 std_Omega^2 0
0 0 0 0 0 std_i^2];
GDgG = G*Dg*G';
% std_x = 10 / sqrt(dTmod) ;
% std_V = 0.01 / sqrt(dTmod);
Dn = [std_x^2 0 0 0 0 0
0 std_x^2 0 0 0 0
0 0 std_x^2 0 0 0
0 0 0 std_V^2 0 0
0 0 0 0 std_V^2 0
0 0 0 0 0 std_V^2];
c = [1 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 0];
for i = 1:Nmod
x0_izm = Xist.x0(i) + std_x * randn(1,1);
y0_izm = Xist.y0(i) + std_x * randn(1,1);
z0_izm = Xist.z0(i) + std_x * randn(1,1);
Vx_izm = Xist.d_x0(i) + std_V * randn(1,1);
Vy_izm = Xist.d_y0(i) + std_V * randn(1,1);
Vz_izm = Xist.d_z0(i) + std_V * randn(1,1);
% For testing and debug
% Xextr.X(1) = Xest_won.e(i);
% Xextr.X(3) = Xest_won.p(i)/p_mult;
% Xextr.X(5) = Xest_won.theta(i);
% Xextr.X(7) = Xest_won.omega(i);
% Xextr.X(9) = Xest_won.Omega(i);
% Xextr.X(11) = Xest_won.i(i);
e = Xextr.X(1);
p = Xextr.X(3) * p_mult;
theta = Xextr.X(5);
omega = Xextr.X(7);
Omega = Xextr.X(9);
i0 = Xextr.X(11);
munapi = sqrt(mu_earth / p);
u = theta + omega;
oec = 1 + e*cos(theta);
es = e*sin(theta);
Sc_x = cos(u)*cos(Omega) - sin(u)*sin(Omega)*cos(i0);
Sc_y = cos(u)*sin(Omega) + sin(u)*cos(Omega)*cos(i0);
Sc_z = sin(u)*sin(i0);
Sc_Vx = sin(u)*cos(Omega) + cos(u)*sin(Omega)*cos(i0);
Sc_Vy = sin(u)*sin(Omega) - cos(u)*cos(Omega)*cos(i0);
Sc_Vz = cos(u)*sin(i0);
[x0_extr y0_extr z0_extr Vx_extr Vy_extr Vz_extr] = ...
get_vector_XV( e, p, theta, omega, Omega, i0);
dY = [x0_izm; y0_izm; z0_izm; Vx_izm; Vy_izm; Vz_izm] - ...
[x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr];
S0 = nan(6,6);
% Discriminator for e
S0(1, 1) = - p * Sc_x / (1+e*cos(theta))^2 * cos(theta);
S0(2, 1) = - p * Sc_y / (1+e*cos(theta))^2 * cos(theta);
S0(3, 1) = - p * Sc_z / (1+e*cos(theta))^2 * cos(theta);
S0(4, 1) = munapi * (sin(theta)*Sc_x - cos(theta)*Sc_Vx);
S0(5, 1) = munapi * (sin(theta)*Sc_y - cos(theta)*Sc_Vy);
S0(6, 1) = munapi * (sin(theta)*Sc_z + cos(theta)*Sc_Vz);
% Discriminator for p
S0(1, 2) = Sc_x/oec;
S0(2, 2) = Sc_y/oec;
S0(3, 2) = Sc_z/oec;
S0(4, 2) = - 0.5 * 1/p * munapi * (es *Sc_x - oec*Sc_Vx);
S0(5, 2) = - 0.5 * 1/p * munapi * (es *Sc_y - oec*Sc_Vy);
S0(6, 2) = - 0.5 * 1/p * munapi * (es *Sc_z + oec*Sc_Vz);
S0(:,2) = S0(:,2) * p_mult;
% Discriminator for theta
Sc_x_theta = - sin(u)*cos(Omega) - cos(u)*sin(Omega)*cos(i0);
Sc_y_theta = - sin(u)*sin(Omega) + cos(u)*cos(Omega)*cos(i0);
Sc_z_theta = cos(u)*sin(i0);
Sc_Vx_theta = cos(u)*cos(Omega) - sin(u)*sin(Omega)*cos(i0);
Sc_Vy_theta = cos(u)*sin(Omega) + sin(u)*cos(Omega)*cos(i0);
Sc_Vz_theta = -sin(u)*sin(i0);
S0(1, 3) = e*Sc_x*p / oec^2 * sin(theta) + p / oec * Sc_x_theta;
S0(2, 3) = e*Sc_y*p / oec^2 * sin(theta) + p / oec * Sc_y_theta;
S0(3, 3) = e*Sc_z*p / oec^2 * sin(theta) + p / oec * Sc_z_theta;
S0(4, 3) = -munapi*Sc_Vx_theta;
S0(5, 3) = -munapi*Sc_Vy_theta;
S0(6, 3) = munapi*Sc_Vz_theta;
% dx = 0.001;
% [x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ...
% get_vector_XV( e, p, theta+dx, omega, Omega, i0);
% dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ...
% [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr];
%
% S0(1, 3) = dS(1) / dx;
% S0(2, 3) = dS(2) / dx;
% S0(3, 3) = dS(3) / dx;
% S0(4, 3) = dS(4) / dx;
% S0(5, 3) = dS(5) / dx;
% S0(6, 3) = dS(6) / dx;
% Discriminator for omega
Sc_x_omega = Sc_x_theta;
Sc_y_omega = Sc_y_theta;
Sc_z_omega = Sc_z_theta;
Sc_Vx_omega = Sc_Vx_theta;
Sc_Vy_omega = Sc_Vy_theta;
Sc_Vz_omega = Sc_Vz_theta;
S0(1,4) = p / oec * Sc_x_omega;
S0(2,4) = p / oec * Sc_y_omega;
S0(3,4) = p / oec * Sc_z_omega;
S0(4,4) = munapi * (e*sin(omega)*Sc_x_omega - oec*Sc_Vx_omega);
S0(5,4) = munapi * (e*sin(omega)*Sc_y_omega - oec*Sc_Vy_omega);
S0(6,4) = munapi * (e*sin(omega)*Sc_z_omega + oec*Sc_Vz_omega);
S01(i) = S0(1,4);
S02(i) = S0(2,4);
S03(i) = S0(3,4);
S04(i) = S0(4,4);
S05(i) = S0(5,4);
S06(i) = S0(6,4);
dx = 1e-6;
[x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ...
get_vector_XV( e, p, theta, omega+dx, Omega, i0);
dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ...
[x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr];
S0(1, 4) = dS(1) / dx;
S0(2, 4) = dS(2) / dx;
S0(3, 4) = dS(3) / dx;
S0(4, 4) = dS(4) / dx;
S0(5, 4) = dS(5) / dx;
S0(6, 4) = dS(6) / dx;
S01_ist(i) = S0(1,4);
S02_ist(i) = S0(2,4);
S03_ist(i) = S0(3,4);
S04_ist(i) = S0(4,4);
S05_ist(i) = S0(5,4);
S06_ist(i) = S0(6,4);
% Discriminator for Omega
Sc_x_Omega = -cos(u)*sin(Omega) - sin(u)*cos(Omega)*cos(i0);
Sc_y_Omega = cos(u)*cos(Omega) - sin(u)*sin(Omega)*cos(i0);
Sc_z_Omega = 0;
Sc_Vx_Omega = -sin(u)*sin(Omega) + cos(u)*cos(Omega)*cos(i0);
Sc_Vy_Omega = sin(u)*cos(Omega) + cos(u)*sin(Omega)*cos(i0);
Sc_Vz_Omega = 0;
S0(1,5) = p/oec*Sc_x_Omega;
S0(2,5) = p/oec*Sc_y_Omega;
S0(3,5) = 0;
S0(4,5) = munapi * (e*sin(omega)*Sc_x_Omega - oec*Sc_Vx_Omega);
S0(5,5) = munapi * (e*sin(omega)*Sc_y_Omega - oec*Sc_Vy_Omega);
S0(6,5) = 0;
% dx = 0.001;
% [x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ...
% get_vector_XV( e, p, theta, omega, Omega+dx, i0);
% dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ...
% [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr];
%
% S0(1, 5) = dS(1) / dx;
% S0(2, 5) = dS(2) / dx;
% S0(3, 5) = dS(3) / dx;
% S0(4, 5) = dS(4) / dx;
% S0(5, 5) = dS(5) / dx;
% S0(6, 5) = dS(6) / dx;
% Discriminator for i
Sc_x_i = sin(u)*sin(Omega)*sin(i0);
Sc_y_i = -sin(u)*cos(Omega)*sin(i0);
Sc_z_i = sin(u)*cos(i0);
Sc_Vx_i = -cos(u)*sin(Omega)*sin(i0);
Sc_Vy_i = cos(u)*cos(Omega)*sin(i0);
Sc_Vz_i = cos(u)*cos(i0);
S0(1, 6) = p/oec * Sc_x_i;
S0(2, 6) = p/oec * Sc_y_i;
S0(3, 6) = p/oec * Sc_z_i;
S0(4, 6) = munapi * (e*sin(theta)*Sc_x_i - oec*Sc_Vx_i);
S0(5, 6) = munapi * (e*sin(theta)*Sc_y_i - oec*Sc_Vy_i);
S0(6, 6) = munapi * (e*sin(theta)*Sc_z_i + oec*Sc_Vz_i);
% dx = 0.001;
% [x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ...
% get_vector_XV( e, p, theta, omega, Omega, i0+dx);
% dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ...
% [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr];
%
% S0(1, 6) = dS(1) / dx;
% S0(2, 6) = dS(2) / dx;
% S0(3, 6) = dS(3) / dx;
% S0(4, 6) = dS(4) / dx;
% S0(5, 6) = dS(5) / dx;
% S0(6, 6) = dS(6) / dx;
S = (c'*S0')';
Dextr = F*Dest*F' + GDgG;
t1 = S'/Dn*S;
t2 = inv(Dextr);
Dest = inv(t1 + t2);
Xest.X = Xextr.X + Dest*S'/Dn*dY;
Xextr.X = F*Xest.X;
if Xest.X(1) > 0;
Xest.e(i) = Xest.X(1);
Xest.theta(i) = mod_pm_pi(Xest.X(5));
Xest.omega(i) = mod_pm_pi(Xest.X(7));
else
Xest.e(i) = -Xest.X(1);
Xest.theta(i) = mod_pm_pi(-Xest.X(5));
Xest.omega(i) = mod_pm_pi(-Xest.X(7));
end
Xest.p(i) = Xest.X(3)*p_mult;
Xest.Omega(i) = mod_pm_pi(Xest.X(9));
Xest.i(i) = mod_pm_pi(Xest.X(11));
[Xest.x0(i) Xest.y0(i) Xest.z0(i) Xest.d_x0(i) Xest.d_y0(i) Xest.d_z0(i)] = ...
get_vector_XV( Xest.e(i), Xest.p(i), Xest.theta(i), Xest.omega(i), Xest.Omega(i), Xest.i(i) );
[Xest.x(i) Xest.y(i) Xest.z(i) tmp1 tmp2 tmp3] = ...
get_vector_XV( Xest.e(i), Xest.p(i), Xest.theta(i), Xest.omega(i), Xest.Omega(i) - omega_e*tmod(i), Xest.i(i) );
Xest.d_e(i) = Xest.X(2);
Xest.d_p(i) = Xest.X(4)*p_mult;
Xest.d_theta(i) = Xest.X(6);
Xest.d_omega(i) = Xest.X(8);
Xest.d_Omega(i) = Xest.X(10);
Xest.d_i(i) = Xest.X(12);
if ~mod(i, fix(Nmod/100))
set(handles.txt_Track, 'String', sprintf('%.0f %%', i/Nmod*100));
end
drawnow
pause(0.01);
end
set(handles.txt_Track, 'String', sprintf('%.0f %%', 100));
set(handles.pb_Track, 'Enable', 'on');
set(handles.pan_Orbital_Elements, 'Visible', 'off');
set(handles.pan_Tracking, 'Visible', 'on');
% Output results to form
for i = 1:5
for j = 1:5
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'DrawMode', 'fast');
plot_axes_Track(handles, 0, [num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Tag', ['axes_Track_' num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_Track_ButtonDownFcn'',hObject,eventdata,guidata(hObject))'));
end
end
plot_axes_Earth(handles, 0);
draw_Errors(handles);
end
%> @brief Tracking alghorithm
%> @param handles Main handles struct
%> ======================================================================
function track_with_noise( handles, std_x, std_V )
set(handles.pb_Track, 'Enable', 'off');
pause(0.01);
globals;
% Filter step
T = dTmod;
% Evolutionary matrix
F = [1 T 0 0 0 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0 0 0 0 0;
0 0 1 T 0 0 0 0 0 0 0 0;
0 0 0 1 0 0 0 0 0 0 0 0;
0 0 0 0 1 T 0 0 0 0 0 0;
0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 0 0 0 1 T 0 0 0 0;
0 0 0 0 0 0 0 1 0 0 0 0;
0 0 0 0 0 0 0 0 1 T 0 0;
0 0 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 0 0 0 1 T;
0 0 0 0 0 0 0 0 0 0 0 1];
p_mult = 5e7; % To simplify matrix calculations - reducing the dynamic range
% Xextr.X = [Xest_won.e(1); Xest_won.p(1)/p_mult; Xest_won.theta(1); 0; Xest_won.omega(1); 0; Xest_won.Omega(1); 0; Xest_won.i(1); 0];
% Xextr.X = [0.01; 0; Xest_won.p(1)/p_mult; 0; 1; Xest_won.d_theta(1)*1.1; 0; 0; 0; Xest_won.d_Omega(1)*0.9; Xest_won.i(1)*0.9; Xest_won.d_i(1)];
Xextr.X = [0.01; 0; 7e6/p_mult; 0; 1; Xest_won.d_theta(1)*1.1; 0; 0; 0; Xest_won.d_Omega(1)*0.9; Xest_won.i(1)*0.9; Xest_won.d_i(1)];
Xest.X = Xextr.X;
% RMS of shaping noises
std_e = 5e-6 / 15*dTmod;
std_p = 1 / 15*dTmod / p_mult; % [m]
std_theta = 1e-5 / 15*dTmod; % [rad]
std_omega = 1e-6 / 15*dTmod; % [rad]
std_Omega = 1e-8 / 15*dTmod; % [rad]
std_i = 1e-8 / 15*dTmod; % [rad]
Dest = [std_e^2*1e1 0 0 0 0 0 0 0 0 0 0 0
0 std_e^2*1e2 0 0 0 0 0 0 0 0 0 0
0 0 std_p^2*1e3 0 0 0 0 0 0 0 0 0
0 0 0 std_p^2*1e4 0 0 0 0 0 0 0 0
0 0 0 0 std_theta^2*1e2 0 0 0 0 0 0 0
0 0 0 0 0 std_theta^2*1e0 0 0 0 0 0 0
0 0 0 0 0 0 std_omega^2*1e2 0 0 0 0 0
0 0 0 0 0 0 0 std_omega^2*1e2 0 0 0 0
0 0 0 0 0 0 0 0 std_Omega^2*1e2 0 0 0
0 0 0 0 0 0 0 0 0 std_Omega^2*1e2 0 0
0 0 0 0 0 0 0 0 0 0 std_i^2*1e2 0
0 0 0 0 0 0 0 0 0 0 0 std_i^2*1e2];
G = [0 0 0 0 0 0;
1 0 0 0 0 0;
0 0 0 0 0 0;
0 1 0 0 0 0;
0 0 0 0 0 0;
0 0 1 0 0 0;
0 0 0 0 0 0;
0 0 0 1 0 0;
0 0 0 0 0 0;
0 0 0 0 1 0;
0 0 0 0 0 0;
0 0 0 0 0 1];
Dg = [std_e^2 0 0 0 0 0
0 std_p^2 0 0 0 0
0 0 std_theta^2 0 0 0
0 0 0 std_omega^2 0 0
0 0 0 0 std_Omega^2 0
0 0 0 0 0 std_i^2];
GDgG = G*Dg*G';
% std_x = 10 / sqrt(dTmod) ;
% std_V = 0.01 / sqrt(dTmod);
Dn = [std_x^2 0 0 0 0 0
0 std_x^2 0 0 0 0
0 0 std_x^2 0 0 0
0 0 0 std_V^2 0 0
0 0 0 0 std_V^2 0
0 0 0 0 0 std_V^2];
c = [1 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 0 0 0;
0 0 0 0 1 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 0];
for i = 1:Nmod
x0_izm = Xist.x0(i) + std_x * randn(1,1);
y0_izm = Xist.y0(i) + std_x * randn(1,1);
z0_izm = Xist.z0(i) + std_x * randn(1,1);
Vx_izm = Xist.d_x0(i) + std_V * randn(1,1);
Vy_izm = Xist.d_y0(i) + std_V * randn(1,1);
Vz_izm = Xist.d_z0(i) + std_V * randn(1,1);
% For testing and debug
% Xextr.X(1) = Xest_won.e(i);
% Xextr.X(3) = Xest_won.p(i)/p_mult;
% Xextr.X(5) = Xest_won.theta(i);
% Xextr.X(7) = Xest_won.omega(i);
% Xextr.X(9) = Xest_won.Omega(i);
% Xextr.X(11) = Xest_won.i(i);
e = Xextr.X(1);
p = Xextr.X(3) * p_mult;
theta = Xextr.X(5);
omega = Xextr.X(7);
Omega = Xextr.X(9);
i0 = Xextr.X(11);
munapi = sqrt(mu_earth / p);
u = theta + omega;
oec = 1 + e*cos(theta);
es = e*sin(theta);
Sc_x = cos(u)*cos(Omega) - sin(u)*sin(Omega)*cos(i0);
Sc_y = cos(u)*sin(Omega) + sin(u)*cos(Omega)*cos(i0);
Sc_z = sin(u)*sin(i0);
Sc_Vx = sin(u)*cos(Omega) + cos(u)*sin(Omega)*cos(i0);
Sc_Vy = sin(u)*sin(Omega) - cos(u)*cos(Omega)*cos(i0);
Sc_Vz = cos(u)*sin(i0);
[x0_extr y0_extr z0_extr Vx_extr Vy_extr Vz_extr] = ...
get_vector_XV( e, p, theta, omega, Omega, i0);
dY = [x0_izm; y0_izm; z0_izm; Vx_izm; Vy_izm; Vz_izm] - ...
[x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr];
S0 = nan(6,6);
% Discriminator for e
S0(1, 1) = - p * Sc_x / (1+e*cos(theta))^2 * cos(theta);
S0(2, 1) = - p * Sc_y / (1+e*cos(theta))^2 * cos(theta);
S0(3, 1) = - p * Sc_z / (1+e*cos(theta))^2 * cos(theta);
S0(4, 1) = munapi * (sin(theta)*Sc_x - cos(theta)*Sc_Vx);
S0(5, 1) = munapi * (sin(theta)*Sc_y - cos(theta)*Sc_Vy);
S0(6, 1) = munapi * (sin(theta)*Sc_z + cos(theta)*Sc_Vz);
% Discriminator for p
S0(1, 2) = Sc_x/oec;
S0(2, 2) = Sc_y/oec;
S0(3, 2) = Sc_z/oec;
S0(4, 2) = - 0.5 * 1/p * munapi * (es *Sc_x - oec*Sc_Vx);
S0(5, 2) = - 0.5 * 1/p * munapi * (es *Sc_y - oec*Sc_Vy);
S0(6, 2) = - 0.5 * 1/p * munapi * (es *Sc_z + oec*Sc_Vz);
S0(:,2) = S0(:,2) * p_mult;
% Discriminator for theta
Sc_x_theta = - sin(u)*cos(Omega) - cos(u)*sin(Omega)*cos(i0);
Sc_y_theta = - sin(u)*sin(Omega) + cos(u)*cos(Omega)*cos(i0);
Sc_z_theta = cos(u)*sin(i0);
Sc_Vx_theta = cos(u)*cos(Omega) - sin(u)*sin(Omega)*cos(i0);
Sc_Vy_theta = cos(u)*sin(Omega) + sin(u)*cos(Omega)*cos(i0);
Sc_Vz_theta = -sin(u)*sin(i0);
S0(1, 3) = e*Sc_x*p / oec^2 * sin(theta) + p / oec * Sc_x_theta;
S0(2, 3) = e*Sc_y*p / oec^2 * sin(theta) + p / oec * Sc_y_theta;
S0(3, 3) = e*Sc_z*p / oec^2 * sin(theta) + p / oec * Sc_z_theta;
S0(4, 3) = -munapi*Sc_Vx_theta;
S0(5, 3) = -munapi*Sc_Vy_theta;
S0(6, 3) = munapi*Sc_Vz_theta;
% dx = 0.001;
% [x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ...
% get_vector_XV( e, p, theta+dx, omega, Omega, i0);
% dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ...
% [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr];
%
% S0(1, 3) = dS(1) / dx;
% S0(2, 3) = dS(2) / dx;
% S0(3, 3) = dS(3) / dx;
% S0(4, 3) = dS(4) / dx;
% S0(5, 3) = dS(5) / dx;
% S0(6, 3) = dS(6) / dx;
% Discriminator for omega
Sc_x_omega = Sc_x_theta;
Sc_y_omega = Sc_y_theta;
Sc_z_omega = Sc_z_theta;
Sc_Vx_omega = Sc_Vx_theta;
Sc_Vy_omega = Sc_Vy_theta;
Sc_Vz_omega = Sc_Vz_theta;
S0(1,4) = p / oec * Sc_x_omega;
S0(2,4) = p / oec * Sc_y_omega;
S0(3,4) = p / oec * Sc_z_omega;
S0(4,4) = munapi * (e*sin(omega)*Sc_x_omega - oec*Sc_Vx_omega);
S0(5,4) = munapi * (e*sin(omega)*Sc_y_omega - oec*Sc_Vy_omega);
S0(6,4) = munapi * (e*sin(omega)*Sc_z_omega + oec*Sc_Vz_omega);
S01(i) = S0(1,4);
S02(i) = S0(2,4);
S03(i) = S0(3,4);
S04(i) = S0(4,4);
S05(i) = S0(5,4);
S06(i) = S0(6,4);
dx = 1e-6;
[x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ...
get_vector_XV( e, p, theta, omega+dx, Omega, i0);
dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ...
[x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr];
S0(1, 4) = dS(1) / dx;
S0(2, 4) = dS(2) / dx;
S0(3, 4) = dS(3) / dx;
S0(4, 4) = dS(4) / dx;
S0(5, 4) = dS(5) / dx;
S0(6, 4) = dS(6) / dx;
S01_ist(i) = S0(1,4);
S02_ist(i) = S0(2,4);
S03_ist(i) = S0(3,4);
S04_ist(i) = S0(4,4);
S05_ist(i) = S0(5,4);
S06_ist(i) = S0(6,4);
% Discriminator for Omega
Sc_x_Omega = -cos(u)*sin(Omega) - sin(u)*cos(Omega)*cos(i0);
Sc_y_Omega = cos(u)*cos(Omega) - sin(u)*sin(Omega)*cos(i0);
Sc_z_Omega = 0;
Sc_Vx_Omega = -sin(u)*sin(Omega) + cos(u)*cos(Omega)*cos(i0);
Sc_Vy_Omega = sin(u)*cos(Omega) + cos(u)*sin(Omega)*cos(i0);
Sc_Vz_Omega = 0;
S0(1,5) = p/oec*Sc_x_Omega;
S0(2,5) = p/oec*Sc_y_Omega;
S0(3,5) = 0;
S0(4,5) = munapi * (e*sin(omega)*Sc_x_Omega - oec*Sc_Vx_Omega);
S0(5,5) = munapi * (e*sin(omega)*Sc_y_Omega - oec*Sc_Vy_Omega);
S0(6,5) = 0;
% dx = 0.001;
% [x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ...
% get_vector_XV( e, p, theta, omega, Omega+dx, i0);
% dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ...
% [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr];
%
% S0(1, 5) = dS(1) / dx;
% S0(2, 5) = dS(2) / dx;
% S0(3, 5) = dS(3) / dx;
% S0(4, 5) = dS(4) / dx;
% S0(5, 5) = dS(5) / dx;
% S0(6, 5) = dS(6) / dx;
% Discriminator for i
Sc_x_i = sin(u)*sin(Omega)*sin(i0);
Sc_y_i = -sin(u)*cos(Omega)*sin(i0);
Sc_z_i = sin(u)*cos(i0);
Sc_Vx_i = -cos(u)*sin(Omega)*sin(i0);
Sc_Vy_i = cos(u)*cos(Omega)*sin(i0);
Sc_Vz_i = cos(u)*cos(i0);
S0(1, 6) = p/oec * Sc_x_i;
S0(2, 6) = p/oec * Sc_y_i;
S0(3, 6) = p/oec * Sc_z_i;
S0(4, 6) = munapi * (e*sin(theta)*Sc_x_i - oec*Sc_Vx_i);
S0(5, 6) = munapi * (e*sin(theta)*Sc_y_i - oec*Sc_Vy_i);
S0(6, 6) = munapi * (e*sin(theta)*Sc_z_i + oec*Sc_Vz_i);
% dx = 0.001;
% [x0_extr2 y0_extr2 z0_extr2 Vx_extr2 Vy_extr2 Vz_extr2] = ...
% get_vector_XV( e, p, theta, omega, Omega, i0+dx);
% dS = [x0_extr2; y0_extr2; z0_extr2; Vx_extr2; Vy_extr2; Vz_extr2] - ...
% [x0_extr; y0_extr; z0_extr; Vx_extr; Vy_extr; Vz_extr];
%
% S0(1, 6) = dS(1) / dx;
% S0(2, 6) = dS(2) / dx;
% S0(3, 6) = dS(3) / dx;
% S0(4, 6) = dS(4) / dx;
% S0(5, 6) = dS(5) / dx;
% S0(6, 6) = dS(6) / dx;
S = (c'*S0')';
Dextr = F*Dest*F' + GDgG;
t1 = S'/Dn*S;
t2 = inv(Dextr);
Dest = inv(t1 + t2);
Xest.X = Xextr.X + Dest*S'/Dn*dY;
Xextr.X = F*Xest.X;
if Xest.X(1) > 0;
Xest.e(i) = Xest.X(1);
Xest.theta(i) = mod_pm_pi(Xest.X(5));
Xest.omega(i) = mod_pm_pi(Xest.X(7));
else
Xest.e(i) = -Xest.X(1);
Xest.theta(i) = mod_pm_pi(-Xest.X(5));
Xest.omega(i) = mod_pm_pi(-Xest.X(7));
end
Xest.p(i) = Xest.X(3)*p_mult;
Xest.Omega(i) = mod_pm_pi(Xest.X(9));
Xest.i(i) = mod_pm_pi(Xest.X(11));
[Xest.x0(i) Xest.y0(i) Xest.z0(i) Xest.d_x0(i) Xest.d_y0(i) Xest.d_z0(i)] = ...
get_vector_XV( Xest.e(i), Xest.p(i), Xest.theta(i), Xest.omega(i), Xest.Omega(i), Xest.i(i) );
[Xest.x(i) Xest.y(i) Xest.z(i) tmp1 tmp2 tmp3] = ...
get_vector_XV( Xest.e(i), Xest.p(i), Xest.theta(i), Xest.omega(i), Xest.Omega(i) - omega_e*tmod(i), Xest.i(i) );
Xest.d_e(i) = Xest.X(2);
Xest.d_p(i) = Xest.X(4)*p_mult;
Xest.d_theta(i) = Xest.X(6);
Xest.d_omega(i) = Xest.X(8);
Xest.d_Omega(i) = Xest.X(10);
Xest.d_i(i) = Xest.X(12);
if ~mod(i, fix(Nmod/100))
set(handles.txt_Track, 'String', sprintf('%.0f %%', i/Nmod*100));
end
drawnow
pause(0.01);
end
set(handles.txt_Track, 'String', sprintf('%.0f %%', 100));
set(handles.pb_Track, 'Enable', 'on');
set(handles.pan_Orbital_Elements, 'Visible', 'off');
set(handles.pan_Tracking, 'Visible', 'on');
% Output results to form
for i = 1:5
for j = 1:5
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'DrawMode', 'fast');
plot_axes_Track(handles, 0, [num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'Tag', ['axes_Track_' num2str(i) num2str(j)]);
set(eval(['handles.axes_Track_' num2str(i) num2str(j)]), 'ButtonDownFcn', str2func('@(hObject,eventdata)fig_main(''axes_Track_ButtonDownFcn'',hObject,eventdata,guidata(hObject))'));
end
end
plot_axes_Earth(handles, 0);
draw_Errors(handles);
end
[править] U1.m
function M = U1(x)
M = [1 0 0;
0 cos(x) sin(x);
0 -sin(x) cos(x)];
M = [1 0 0;
0 cos(x) sin(x);
0 -sin(x) cos(x)];
[править] U2.m
function M = U2(x)
M = [cos(x) 0 -sin(x);
0 1 0;
sin(x) 0 cos(x)];
M = [cos(x) 0 -sin(x);
0 1 0;
sin(x) 0 cos(x)];
[править] U3.m
function M = U3(x)
M = [cos(x) sin(x) 0;
-sin(x) cos(x) 0;
0 0 1];
M = [cos(x) sin(x) 0;
-sin(x) cos(x) 0;
0 0 1];