Appendix A. Listing — различия между версиями

Материал из SRNS
Перейти к: навигация, поиск
м (переименовал «Тестовая страница» в «Appendix A. Listing»)
 
(не показаны 9 промежуточных версий 2 участников)
Строка 1: Строка 1:
−
Проверка ТеХа
+
__NOTOC__
−
<math>E = m \cdot c^2 </math>
+
=== main.m ===
  
−
Русский $\cdot$:
+
<source lang="matlab">try
−
<math>\text{Б} = \sqrt{\text{Б}^2}</math>
+
    close(handle_fig_main); % Close old output form
 +
end
  
−
<math>N_0 = k \cdot T_n</math>
+
close all
 +
clear
 +
clc
  
−
<math>a^2 = \frac{c}{d_1^2}</math>
+
globals;
  
−
== Geshi ==
+
addpath([pwd '/func/tracking']); % Functions for tracking algorithm
 +
addpath([pwd '/func/solve']); % Functions for solving without noise
 +
addpath([pwd '/func/interface']); % Functions for interface
  
−
<syntaxhighlight lang = "c">
+
Tmod = 3.2*60*60; %[s], duration of the simulation
−
int i = 0;
+
−
</syntaxhighlight>
+
  
−
== Source ==
+
% 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
  
−
<source lang = "c">
+
% Load true trajectory of SV
−
int i = 0;
+
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>
 
</source>
  
  
−
Ссылка на ico-файл:
+
===fig_main.m===
−
[[File:20110403_CPS.ico]]
+
  
−
[[File:20110329_NII_KP_GPSCA_result.PNG|500px|center|thumb|Результат чего-то там]]
+
<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
  
−
[[File:St Moritz Muottas.jpg|thumb|St Moritz Muottas]]
+
if nargout
 +
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
 +
else
 +
    gui_mainfcn(gui_State, varargin{:});
 +
end
 +
% End initialization code - DO NOT EDIT
  
−
[[File:Wikilog.png|thumb|Wikilog logo]]
 
−
[[File:Wikilog.png|Wikilog logo]]
 
  
−
[[File:20110329_NII_KP_GPSCA_result.PNG|thumb|Wikilog logo thumb]]
+
% --- Executes just before fig_main is made visible.
−
[[File:20110329_NII_KP_GPSCA_result.PNG|Wikilog logo]]
+
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


[править] 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


[править] 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');


[править] 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


[править] next_hF.m

function hF_cont = next_hF()
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


[править] 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


[править] 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


[править] 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


[править] 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


[править] 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


[править] mod_pm_pi.m

function y = mod_pm_pi( x )
%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;


[править] 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


[править] U1.m

function M = U1(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)];


[править] U3.m

function M = U3(x)

M = [cos(x)     sin(x)  0;
     -sin(x)    cos(x)  0;
     0          0       1];
Источник — «https://www.srns.ru/index.php?title=Appendix_A._Listing&oldid=6677»
Персональные инструменты
Пространства имён

Варианты
Действия
SRNS Wiki
Рабочие журналы
Приватный файлсервер
QNAP Сервер
Инструменты