Matlab拟合椭圆

1 简介

Matlab拟合椭圆​

2 完整代码

function result = ellipseFit4HC(x,y,options)%ellipseFit4HC Estimates the ellipse parameters, based on N pairs of%     measured (noisy) data (x and y), together with their statistical%     uncertainties (standard errors).%% SYNTAX:%    result = ellipseFit4HC(x,y,options)%% INPUT:%     x        - N-dimensional vector of x-signal measurements; %     y        - N-dimensional vector of y-signal measurements;%     options  - structure with the following parameters:%              - verbose: logical variable, flag for TABLE output of the%                estimated parameters (default value is true);%              - isplot: logical variable, flag for graphical output%                (default value is true);%              - coverageFactor: coverage factor for computing the inteval%                estimators (default value is 2);%              - alpha: nominal significance level for computing the%                (1-alpha)-quantiles (coverage factors) of the inteval%                estimators (default value is alpha = [], i.e. the %                options.coverageFactor is used);%              - tolerance: tolerance for convergence (default is 1e-12);%              - maxloops: maximum number of iterations (default is 100);%              - correlation: known value of the correlation coefficient, %                rho = corr(x,y), while assuming var(x) = var(y) = sigma2,%                (default value is rho = 0);%              - displconst: displacement constant (default value is %                633.3*1000/(4*pi));%              - displunit: displacement constant (default value is %                picometers [pm]);%              - smoothByFFT: smooth the original measurements (x,y)%                by using the significant FFT frequencies.%% OUPUT:%     result   - structure with detailed information on estimated%                parameters.%% EXAMPLE 1: (Fit the ellipse for generated measurements x and y)%     alpha0 = 0; beta0 = 0;        % true ellipse center [0,0]%     alpha1 = 1; beta1 = 0.75;     % true amplitudes of x and y signals %     phi0 = pi/3;                  % phase shift%     X = @(t) alpha0 + alpha1 * cos(t);%     Y = @(t) beta0 + beta1 * sin(t + phi0);%     sigma = 0.05;                 % true measurement error STD%     N = 50;                       % No. of observations (x,y)%     Ncycles = 0.8;                % No. of whole ellipse cycles%     phi = Ncycles*(2*pi)*sort(rand(N,1)); % true phases %     x = X(phi) + sigma*randn(size(X(phi)));%     y = Y(phi) + sigma*randn(size(Y(phi)));%     result = ellipseFit4HC(x,y);%% EXAMPLE 2: (Fit the ellipse for generated measurements x and y)%     alpha0 = 0; beta0 = 0;        % true ellipse center [0,0]%     alpha1 = 1; beta1 = 1;        % true amplitudes%     phi0 = pi/10;                 % phase shift%     X = @(t) alpha0 + alpha1 * cos(t);%     Y = @(t) beta0 + beta1 * sin(t + phi0);%     sigma = 0.05; N = 1000; phi = (2*pi)*((1:N)')/N; %     x = X(phi) + sigma*randn(size(X(phi)));%     y = Y(phi) + sigma*randn(size(Y(phi)));%     result = ellipseFit4HC(x,y)%     disp(result.TABLE_Displacements)%% EXAMPLE 3: (Ellipse fit based on N = 100000 interferometer measurements)%     load InterferometerData%     x = data(:,1) / max(data(:,1));%     y = data(:,2) / max(data(:,1));%     result = ellipseFit4HC(x,y)%     disp(result.TABLE_Displacements(1:20,:))%% REMARKS:%     In particular, ellipseFit4HC can be useful for uncertaity evaluation%     of the estimated phases and/or displacements, based on quadrature%     homodyne interferometer measurements (with the Heydemann Correction%     applied). %%     The *Heydemann Correction* is used to evaluate the phase in homodyne%     interferometer applications to correct the interferometer%     nonlinearities), for more details see e.g. Koening et al (2014) and%     Wu, Su and Peng (1996).% %     Here we assume that the measurement errors for x and y are%     independent (optionally correlated, with known correlation%     coefficient rho), with zero mean and common variance sigma^2. %%     The standard deviation of the measurement errors, sigma, is assumed%     to be small, such that the measurements are relatively close to the%     true, however unobservable ellipse curve, as is the case for typical%     interferometry measurements.% %     Moreover, due to numerical stability of the algorithm, it is%     reasonable to consider normalized measurements (x,y), i.e. such that%     the length of the main semiaxis of the fitted ellipse is close to 1.% %     The standard algebraic parametrization of ellipse, (A,B,C,D,E,F), see%     e.g. Chernov & Wijewickrema (2013), is %       A*x^2 + 2*B*x*y + C*y^2 + 2*D*x + 2*E*y + F = 0 %     or (xc,yc,a,b,alpha), in geometric parametrization of the ellipse, of%     the following form:%      [(x-xc)*cos(alpha) + (y-yc)*sin(alpha)]^2 / a^2 +%         ... [(x-xc)*sin(alpha) + (y-yc)*cos(alpha)]^2 / b^2 = 1. %     where -pi/2 < alpha < pi/2, xc, yc denote the coordinates of the%     ellipse center, and a, b are the ellipse semiaxis.% %     Alternatively, one can define the ellipse also in the following%     parametric form:%      x(t) = xc + a * cos(t) * cos(alpha) - b * sin(t) * sin(alpha)%      y(t) = yc + a * cos(t) * sin(alpha) + b * sin(t) * cos(alpha).% %     However, here we consider the following algebraic parametrization of%     the ellipse, (B,C,D,F,G), as it is typically used in the field of%     interferometry, see e.g. Wu, Su and Peng (1996):%      x^2 + B*y^2 + C*x*y + D*x + F*y + G = 0,%     and the geometric parametrization, (alpha_0,alpha_1,beta_0, beta_1,%     phi_0) of the form:%      x(phi) = alpha_0 + alpha_1 * cos(phi)%      y(phi) = beta_0 + beta_1 * sin(phi + phi_0).%     where  -pi/2 < phi0 < pi/2 is the phase offset, alpha_0, beta_0%     denote the coordinates of the ellipse center (offsets), and alpha_1,%     beta_1 are the signal amplitudes.% %     Fitting ellipse based on measurements (x,y) is a non-linear problem.%     The presented approach is based on an approximate method which is%     correct to the first order. It is an iterative procedure based on%     subsequent first order Taylor expansions (linearizations) of the%     originally nonlinear model.% %     The algorithm estimates the locally approximate BLUEs (Best Linear%     Unbiased Estimators) of the ellipse parameters (B,C,D,F,G), the BLUEs%     of the true signal values mu and nu (the values on the fitted%     ellipse) of the measurements (x,y), together with their covariance%     matrix, as suggested in Koening et al (2014).% %     This is based on iterative linearizations of the originally nonlinear%     model with nonlinear constraints on the model parameters. For more%     details see Kubacek (1988, p.152).% %     Based on that, ellipseFit4HC estimates also the geometric%     parameters (alpha_0, alpha_1, beta_0, beta_1, phi_0) and the%     N-dimensional vector of phases phi (and/or displacements) together%     with their standard uncertainties computed by using the delta method.%% REFERENCES: % [1] Koening, R., Wimmer, G. and Witkovsky V.: Ellipse fitting by%     linearized nonlinear constrains to demodulate quadrature homodyne%     interferometer signals and to determine the statistical uncertainty%     of the interferometric phase. To appear in Measurement Science and%     Technology, 2014.% [2] Kubacek, L.: Foundations of Estimation Theory. Elsevier, 1988. % [3] Chien-Ming Wu, Ching-Shen Su and Gwo-Sheng Peng. Correction of%     nonlinearity in one-frequency optical interferometry. %     Measurement Science and Technology, 7 (1996), 520?24. % [4] Chernov N., Wijewickrema S.: Algorithms for projecting points onto%     conics. Journal of Computational and Applied Mathematics 251 (2013) %     8?1. % (c) Viktor Witkovsky (witkovsky@savba.sk)% Ver.: 31-Jul-2014 18:27:32%% CHECK THE INPUTS AND OUTPUTStic;narginchk(2, 3);if nargin < 3, options = []; end​if ~isfield(options, 'smoothDataByFFT')    options.smoothDataByFFT = false;end​if ~isfield(options, 'displconst')    options.displconst = 633.3*1000/(4*pi); % unit: [pm]    %options.displconst = 633.3/(4*pi); % unit: [nm] enddisplconst = options.displconst;​if ~isfield(options, 'displunit')    options.displunit = 'picometer [pm]';    %options.displunit = 'nanometer [nm]';enddisplunit = options.displunit;​if ~isfield(options, 'tolerance')    options.tolerance = 1e-12;endtol = options.tolerance;​if ~isfield(options, 'alpha')    options.alpha = [];end​if ~isfield(options, 'maxloops')    options.maxloops = 100;end​if ~isfield(options, 'correlation')    options.correlation = 0;end​if ~isfield(options, 'coverageFactor')    options.coverageFactor = 2;end​if ~isfield(options, 'verbose')    options.verbose = true;endverbose = options.verbose;​if ~isfield(options, 'isplot')    options.isplot = true;endisplot = options.isplot;%% Initialization of the Linear model with Type II constrainsn       = length(x);x       = x(:);y       = y(:);x_data  = x;y_data  = y;% rho is know correlation coefficient rho = corr(x,y) [default rho = 0]% and var(x) = var(y) = sigma^2rho = options.correlation;% smoothDataByFFT IS EXPERIMENTAL OPTION / NOT FOR STANDARD USAGE% Preliminary smooth the data by FFT (fit by using significant frequencies)% This can lead to highly precise estimation of phases, if equidistant% sampling could be reasonably assumedif options.smoothDataByFFT    [x,y,options] = smoothedDataByFFT(x,y,options);end​mu_0     = x;nu_0     = y;​if isempty(options.alpha)    coverageFactor = options.coverageFactor;else    coverageFactor = tinv(1-options.alpha/2,n-5);end%% INITIALIZATIONB0      = [nu_0.^2, mu_0.*nu_0, mu_0, nu_0, ones(n,1)];theta_0 = - (B0'*B0) \ (B0'*mu_0.^2);% nonzero diagonals of A0 = [diag(A0_dg1) diag(A0_dg2)]A0_dg1  = B0 * [0; 0; 2; theta_0(2); theta_0(3)];A0_dg2  = B0 * [0; 0; theta_0(2); 2*theta_0(1); theta_0(4)];c0      = mu_0.^2 + B0 * theta_0;% Vector of observations, say Y,  Y = [x_del; y_del]x_del   = x - mu_0;y_del   = y - nu_0;​A0HA0_inv = sparse(1:n, 1:n, 1./(A0_dg1.^2 + A0_dg2.^2 + 2*rho*A0_dg1.*A0_dg2));% z0 = -(c0 + A0 *[x_del;y_del]) is the right-hand side of equation% [ A0*H*A0' B0; B0' 0] * [lambda; theta_del] = [-(c0 + A0 *[x_del;y_del]); 0]z0       = -(c0 + A0_dg1 .* x_del + A0_dg2 .* y_del);% Estimated parameters (1st iteration)theta_del = (B0' * A0HA0_inv * B0) \ (B0' * A0HA0_inv * z0);lambda    = A0HA0_inv * (z0 - B0 * theta_del);% [mu_del; nu_del] = Y + H*A'*lambda, see Kubacek p. 152mu_del    = x_del + (A0_dg1 + rho*A0_dg2) .* lambda;nu_del    = y_del + (A0_dg2 + rho*A0_dg1) .* lambda;%% ITERATIONSmaxloops  = options.maxloops;criterion = 1;loop      = 0;while criterion > sqrt(n)*tol && loop < maxloops        % LINEARIZATION:    % Update the location (mu_0, nu_0, and theta_0) for linearization    mu_0    = mu_0 + mu_del;    nu_0    = nu_0 + nu_del;    theta_0 = theta_0 + theta_del;        % Update the measurements (x_delta, y_delta) of the linearized model    x_del  = x - mu_0;    y_del  = y - nu_0;        % Update the constraints matrices (A0, B0, c0) of the linearized model    B0     = [nu_0.^2, mu_0.*nu_0, mu_0, nu_0, ones(n,1)];    A0_dg1 = B0 * [0; 0; 2; theta_0(2); theta_0(3)];    A0_dg2 = B0 * [0; 0; theta_0(2); 2*theta_0(1); theta_0(4)];    c0     = mu_0.^2 + B0 * theta_0;             % ESTIMATION:     % Prepare some useful matrices    A0HA0_inv = sparse(1:n, 1:n, ...                1./(A0_dg1.^2 + A0_dg2.^2 + 2*rho*A0_dg1.*A0_dg2));    z0        = - (c0 + A0_dg1 .* x_del + A0_dg2 .* y_del);    % Estimate the parameters    theta_del = (B0' * A0HA0_inv * B0) \ (B0' * A0HA0_inv * z0);    lambda    = A0HA0_inv * (z0 - B0 * theta_del);    mu_del    = x_del + (A0_dg1 + rho*A0_dg2) .* lambda;    nu_del    = y_del + (A0_dg2 + rho*A0_dg1) .* lambda;        % CONVERGENCE: Update the value of the criterion:    criterion = norm([mu_del; nu_del; theta_del]);    loop      = loop + 1;end%% RESULTS Final estimates% Parameters of the fitted ellipsemu_fit       = mu_0 + mu_del;nu_fit       = nu_0 + nu_del;theta_fit    = theta_0 + theta_del;% Residualsx_res       = x_data - mu_fit;y_res       = y_data - nu_fit;% Residual sum of squares: sse = res' *inv(H)* res, % where H = [I rho*I; rho*I I] sse = (x_res' * x_res + y_res' * y_res - 2*rho* x_res' * y_res) / (1-rho^2);% Estimated standard errorif options.smoothDataByFFT    sigma2  = sse / (2*n-5);else    sigma2  = sse / (n-5);endsigma       = sqrt(sigma2);% Estimated covariance matrix of the ellipse coefficients theta = (BCDFG)theta_cov   = (B0' * A0HA0_inv * B0) \ eye(5);theta_cov   = sigma2 * theta_cov;theta_std   = sqrt(diag(theta_cov));% STDs of xmean_fit and ymean_fit[mu_std,nu_std] = munuStdFun(theta_cov,A0_dg1,A0_dg2,A0HA0_inv,B0,sigma2,rho);% Estimated ellipse parameters (alpha0, beta0, alpha1, beta1, phi0) and their STDs[alpha0_fit,alpha0_std] = alpha0FitFun(theta_fit,theta_cov);[beta0_fit,beta0_std]   = beta0FitFun(theta_fit,theta_cov);[phi0_fit,phi0_std]     = phi0FitFun(theta_fit,theta_cov);[alpha1_fit,alpha1_std] = alpha1FitFun(theta_fit,theta_cov);[beta1_fit,beta1_std]   = beta1FitFun(theta_fit,theta_cov);% Estimated geometric parameters as vectorPARS_fit = [alpha0_fit;beta0_fit;alpha1_fit;beta1_fit;phi0_fit];PARS_std = [alpha0_std;beta0_std;alpha1_std;beta1_std;phi0_std];% Estimated phases phi_i and their STDs[phi_fit,phi_std] = phiFitFun(mu_fit,nu_fit,theta_fit,theta_cov, ...    A0_dg1,A0_dg2,A0HA0_inv,B0,sigma2,rho);phi_std_max = max(phi_std);phi_std_min = min(phi_std);% Estimated displacements (displconst * phi_fit) and their STDsdisplacement_fit = displconst * phi_fit;displacement_std = displconst * phi_std;% Fitted functions for x and yX_fitfun    = @(t) alpha0_fit + alpha1_fit * cos(t);Y_fitfun    = @(t) beta0_fit + beta1_fit * sin(t + phi0_fit);%% TABLE Estimated ellipse parameters: (B, C, D, F, G)TABLE_BCDFG = table;TABLE_BCDFG.Properties.Description = ...    'Estimated Ellipse Parameters (B,C,D,F,G)';TABLE_BCDFG.ESTIMATE = theta_fit;TABLE_BCDFG.STD  = theta_std;TABLE_BCDFG.DF = (n-5)*ones(5,1);TABLE_BCDFG.FACTOR = coverageFactor*ones(5,1);TABLE_BCDFG.LOWER = theta_fit - coverageFactor*theta_std;TABLE_BCDFG.UPPER = theta_fit + coverageFactor*theta_std;TABLE_BCDFG.Properties.RowNames = {'B'  'C'  'D'  'F'  'G'};%% TABLE Estimated ellipse parameters: (alpha0, beta0, alpha1, beta1, phi0)TABLE_EllipsePars = table;TABLE_EllipsePars.Properties.Description = ...    'Estimated Ellipse Parameters (alpha_0, beta_0, alpha_1, beta_1, phi_0)';TABLE_EllipsePars.ESTIMATE = PARS_fit;TABLE_EllipsePars.STD  = PARS_std;TABLE_EllipsePars.DF = (n-5)*ones(5,1);TABLE_EllipsePars.FACTOR = coverageFactor*ones(5,1);TABLE_EllipsePars.LOWER = PARS_fit - coverageFactor*PARS_std;TABLE_EllipsePars.UPPER = PARS_fit + coverageFactor*PARS_std;TABLE_EllipsePars.Properties.RowNames = ...    {'alpha_0' 'beta_0' 'alpha_1' 'beta_1' 'phi_0'};%% TABLE Estimated dispacements: const * phiTABLE_Displacements = table;TABLE_Displacements.Properties.Description = ...    ['Estimated Displacements, ',displunit];TABLE_Displacements.OBSERVATION = (1:n)';TABLE_Displacements.ESTIMATE = displacement_fit;TABLE_Displacements.STD  = displacement_std;TABLE_Displacements.DF = (n-5)*ones(n,1);TABLE_Displacements.FACTOR = coverageFactor*ones(n,1);TABLE_Displacements.LOWER = ...    displacement_fit - coverageFactor*displacement_std;TABLE_Displacements.UPPER = ...    displacement_fit + coverageFactor*displacement_std;%% RESULTS / Set the resultresult.Description = ...    'Ellipse Fit by Iterated Locally Best Linear Unbiased Estimation';% Standard Algebraic Parametrization: Ax^2 + 2Bxy + Cy^2 + 2Dx + 2Ey + F = 0result.ABCDEF_Description = 'A*x^2 + 2*B*x*y + C*y^2 + 2*D*x + 2*E*y + F = 0'; %result.ABCDEF_VarNames = {'x.^2' '2*x.*y' 'y.^2' '2*x' '2*y' '1'};result.ABCDEF_Names = {'A' 'B' 'C' 'D' 'E' 'F'};result.ABCDEF_fit = [1; theta_fit(2)/2; theta_fit(1);...    theta_fit(3)/2; theta_fit(4)/2;theta_fit(5)]';result.ABCDEF_std = [0; theta_std(2)/2; theta_std(1);...    theta_std(3)/2; theta_std(4)/2;theta_std(5)]';% Algebraic Parametrization (Peng-Wu): x^2 + By^2 +  Cxy + Dx + Fy + G = 0result.BCDFG_Description = 'x^2 + B*y^2 +  C*x*y + D*x + F*y + G = 0'; %result.BCDFG_VarNames = {'y.^2' 'x.*y' 'x' 'y' '1'};result.BCDFG_Names = {'B' 'C' 'D' 'F' 'G'};result.BCDFG_fit = theta_fit';result.BCDFG_std = theta_std';result.BCDFG_cov = theta_cov;% Geometric Parametrization (Peng-Wu): (alpha_0, beta_0, alpha_1, beta_1, phi_0)% X(phi) = alpha_0 + alpha_1 * cos(phi)       % Y(phi) = beta_0  + beta_1  * sin(phi + phi_0result.EllipsePars_Description_X = 'X(phi) = alpha_0 + alpha_1 * cos(phi)';result.EllipsePars_Description_Y = 'Y(phi) = beta_0  + beta_1  * sin(phi + phi_0)';result.EllipsePars_Names = {'alpha_0' 'beta_0' 'alpha_1' 'beta_1' 'phi_0'};result.EllipsePars_fit = PARS_fit';result.EllipsePars_std = PARS_std';% Estimated muresult.mu_X_fit = mu_fit;result.mu_X_std = mu_std;% Estimated nuresult.nu_Y_fit = nu_fit;result.nu_Y_std = nu_std;% Fitted functionsresult.X_fitfun = X_fitfun;result.Y_fitfun = Y_fitfun;% Estimated residualsresult.x_res_fit = x_res;result.x_res_std = sqrt(sigma2 - mu_std.^2);result.y_res_fit = y_res;result.y_res_std = sqrt(sigma2 - nu_std.^2);% Estimated sigmaresult.sigma_fit = sigma;result.sigma2_fit = sigma.^2;% Estimated phases phiresult.phi_fit = phi_fit;result.phi_std = phi_std;result.phi_std_max = phi_std_max;result.phi_std_min = phi_std_min;%result.phi_std_mean = mean(phi_std);% Estimated Displacementsresult.Displacement_Description = ...    'Displacement_fit = Displacement_const * phi_fit';result.Displacement_fit = displacement_fit;result.Displacement_std = displacement_std;​result.Displacement_std_max = displconst*phi_std_max;result.Displacement_std_min = displconst*phi_std_min;%result.Displacement_std_mean = displconst*mean(phi_std);result.Displacement_const = displconst;result.Displacement_unit = displunit;% TABLESresult.TABLE_BCDFG = TABLE_BCDFG;result.TABLE_EllipsePars = TABLE_EllipsePars;result.TABLE_Displacements = TABLE_Displacements;% Inforesult.x_data = x_data;result.y_data = y_data;result.used_Correlation_XY = rho;result.options = options;result.criterion = criterion;result.loops = loop;result.tictoc = toc;%% PLOT FIGURE / Fitted Ellipseif isplot    t = linspace(0,2*pi,1000)';    figWidth = 1024; figHeight = 1024; % size in pixels    rect = [10 10 figWidth figHeight];    figure1 = figure('Name','FittedEllipse','OuterPosition', rect);    axes1 = axes('Parent',figure1,'PlotBoxAspectRatio',...        [1 1 1],'DataAspectRatio',[1 1 1]);    box(axes1,'on');    hold(axes1,'all');    plot1 = plot(x_data,y_data,'x', ...        alpha0_fit,beta0_fit,'h', ...        X_fitfun(t),Y_fitfun(t),'--', ...        mu_fit,nu_fit,'o', ...        'LineWidth',2,'MarkerSize',8);    set(plot1(1),'DisplayName','measurements (x,y)');    set(plot1(2),'Color',[0 0 0],'MarkerFaceColor','g', ...        'DisplayName','fitted ellipse center');    set(plot1(3),'Color',[1 0 1],'DisplayName','fitted ellipse');    set(plot1(4),'Color',[1 0 1],'MarkerFaceColor','g', ...        'DisplayName','fitted values (X,Y)',...        'LineWidth',1,'MarkerSize',5);    xlabel('x');    ylabel('y');    legend1 = legend(axes1,'show');    set(legend1,'Location','NorthWest');    gridend%% SHOW TABLESif verbose    disp('  -------------------------------------------------')    disp('  ESTIMATED ELLIPSE PARAMETERS                     ')    disp('  X^2 + B*Y^2 + C*X*Y + D*X + F*Y + G = 0          ')    disp('  -------------------------------------------------')    disp(TABLE_BCDFG)    disp('  -------------------------------------------------')    disp('  ESTIMATED ELLIPSE PARAMETERS                     ')    disp('  X(phi) = alpha_0 + alpha_1 * cos(phi)            ')    disp('  Y(phi) = beta_0  + beta_1  * sin(phi + phi_0     ')    disp('  -------------------------------------------------')    disp(TABLE_EllipsePars)%    disp(TABLE_Displacements)%    disp('  -------------------------------------------------')%    disp('  ESTIMATED DISPLACEMENTS ')%    disp('  -------------------------------------------------')end%% END OF ellipseFit4HCend%% Function ATANVW% function phi = atanvw(nom_y,den_x)% %ATANVW Computes standardized value of the angle phi in interval [0,2*pi]% %       based on arctan of nom/den.% % % (c) Viktor Witkovsky (witkovsky@savba.sk)% % Ver.: 15-Sep-2013 10:40:28% % %% CHECK INPUTS AND OUTPUTS% narginchk(2, 2);% % %%% phi = zeros(size(nom_y));% nomsgn = sign(nom_y);% densgn = sign(den_x);% tmp = (nomsgn == 1 & densgn == 1);% phi(tmp) = atan(abs(nom_y(tmp)./den_x(tmp)));% tmp = (nomsgn == 1 & densgn == -1);% phi(tmp) = pi - atan(abs(nom_y(tmp)./den_x(tmp))) ;% tmp = (nomsgn == -1 & densgn == -1);% phi(tmp) = pi + atan(abs(nom_y(tmp)./den_x(tmp)));% tmp = (nomsgn == -1 & densgn == 1);% phi(tmp) = 2*pi - atan(abs(nom_y(tmp)./den_x(tmp)));% % end%% Function alpha0FitFunfunction [alpha0_fit,alpha0_std,alpha0_grad] = alpha0FitFun(BCDFG,BCDFG_cov)%alpha0FitFun Computes the fitted value alpha0_fit and its uncertainty (STD),%       based on estimated ellipse coefficients BCDFG and their%       estimated covariance matrix BCDFG_cov.% (c) Viktor Witkovsky (witkovsky@savba.sk)% Ver.: 31-Jul-2014 18:27:32% Fitted coefficientsB = BCDFG(1);C = BCDFG(2);D = BCDFG(3);F = BCDFG(4);% G = BCDFG(5);% Fitted value of alpha0 (x-center of the ellipse)DEN = 4*B - C^2;alpha0_fit = (C*F - 2*B*D) / DEN;% Gradientalpha0_grad    = zeros(5,1);alpha0_grad(1) = (2*C*(C*D - 2*F))/DEN^2;alpha0_grad(2) = (C^2*F + 4*B*(-C*D + F))/DEN^2;alpha0_grad(3) = -2*B/DEN;alpha0_grad(4) = C/DEN;% Standard deviation (STD)alpha0_std = sqrt(alpha0_grad' * BCDFG_cov * alpha0_grad);​end%% Function beta0FitFunfunction [beta0_fit,beta0_std,beta0_grad] = beta0FitFun(BCDFG,BCDFG_cov)%beta0FitFun Computes the fitted value beta0_fit and its uncertainty (STD),%       based on estimated ellipse coefficients BCDFG and their%       estimated covariance matrix BCDFG_cov.% (c) Viktor Witkovsky (witkovsky@savba.sk)% Ver.: 31-Jul-2014 18:27:32% Fitted coefficientsB = BCDFG(1);C = BCDFG(2);D = BCDFG(3);F = BCDFG(4);% G = BCDFG(5);% Fitted value of beta0 (y-center of the ellipse)DEN = 4*B - C^2;beta0_fit = (C*D - 2*F) / DEN;% Gradientbeta0_grad    = zeros(5,1);beta0_grad(1) = (-4*C*D + 8*F) / DEN^2;beta0_grad(2) = (4*B*D + C*(C*D - 4*F)) / DEN^2;beta0_grad(3) = C / DEN;beta0_grad(4) = -2 / DEN;% Standard deviation (STD)beta0_std = sqrt(beta0_grad' * BCDFG_cov * beta0_grad);​end%% Function phi0FitFunfunction [phi0_fit,phi0_std,phi0_grad] = phi0FitFun(BCDFG,BCDFG_cov)%phi0FitFun Computes the fitted value phi0_fit and its uncertainty (STD),%       based on estimated ellipse coefficients BCDFG and their%       estimated covariance matrix BCDFG_cov.% (c) Viktor Witkovsky (witkovsky@savba.sk)% Ver.: 31-Jul-2014 18:27:32% Fitted coefficientsB = BCDFG(1);C = BCDFG(2);% D = BCDFG(3);% F = BCDFG(4);% G = BCDFG(5);% Fitted value of phi0 (phase offset of the ellipse)phi0_fit = asin(-C/(2*sqrt(B)));% GradientDEN = sqrt(B) * sqrt(1-C^2/(4*B));phi0_grad    = zeros(5,1);phi0_grad(1) = -1 / (2*DEN);phi0_grad(2) = C / (4*B*DEN);% Standard deviation (STD)phi0_std = sqrt(phi0_grad' * BCDFG_cov * phi0_grad);​end%% Function alpha1FitFunfunction [alpha1_fit,alpha1_std,alpha1_grad] = alpha1FitFun(BCDFG,BCDFG_cov)%alpha1FitFun Computes the fitted value alpha1_fit and its uncertainty (STD),%       based on estimated ellipse coefficients BCDFG and their%       estimated covariance matrix BCDFG_cov.% (c) Viktor Witkovsky (witkovsky@savba.sk)% Ver.: 31-Jul-2014 18:27:32% Fitted coefficientsB = BCDFG(1);C = BCDFG(2);D = BCDFG(3);F = BCDFG(4);G = BCDFG(5);% Fitted value of alpha1 (x-semi-axis of the ellipse)DEN  = 4*B - C^2;DEN2 = sqrt(-C*D*F + F^2 + B*(D^2 - 4*G) + C^2*G);alpha1_fit = (2*sqrt(B) * sqrt(B*D^2 - C*D*F + F^2 - (4*B - C^2)*G)) / DEN;% Gradientalpha1_grad = zeros(5,1);alpha1_grad(1) = (B*(4*C*D*F - 4*F^2 - 2*C^2*(D^2 - 2*G)) - C^2* ...    (-C*D*F + F^2 + C^2*G))/(sqrt(B)*DEN^2*DEN2);alpha1_grad(2) = (sqrt(B)*(4*B*(-D*F + C*(D^2 - 2*G)) + C* ...    (-3*C*D*F + 4*F^2 + 2*C^2*G)))/(DEN^2*DEN2);       alpha1_grad(3) = (sqrt(B)*(2*B*D - C*F)) /(DEN*DEN2);alpha1_grad(4) = sqrt(B)*(-C*D + 2*F)/(DEN*DEN2);alpha1_grad(5) = -sqrt(B) / DEN2;% Standard deviation (STD)alpha1_std = sqrt(alpha1_grad' * BCDFG_cov * alpha1_grad);​end%% Function beta1FitFunfunction [beta1_fit,beta1_std,beta1_grad] = beta1FitFun(BCDFG,BCDFG_cov)%beta1FitFun Computes the fitted value beta1_fit and its uncertainty (STD),%       based on estimated ellipse coefficients BCDFG and their%       estimated covariance matrix BCDFG_cov.% (c) Viktor Witkovsky (witkovsky@savba.sk)% Ver.: 31-Jul-2014 18:27:32% Fitted coefficientsB = BCDFG(1);C = BCDFG(2);D = BCDFG(3);F = BCDFG(4);G = BCDFG(5);% Fitted value of beta1 (y-semi-axis of the ellipse)DEN = 4*B - C^2;DEN2 = sqrt(-C*D*F + F^2 + B*(D^2 - 4*G) + C^2*G);beta1_fit = (2*sqrt(B*D^2 - C*D*F + F^2 - (4*B - C^2)*G))/DEN;% Gradientbeta1_grad = zeros(5,1);beta1_grad(1) = (8*C*D*F - 8*F^2 - 4*B* (D^2 - 4*G) - ...    C^2*(D^2 + 4*G)) /(DEN^2*DEN2);beta1_grad(2) = (4*B*(-D*F + C*(D^2 - 2*G)) + ...    C*(-3*C*D*F + 4*F^2 + 2*C^2*G))/(DEN^2*DEN2);     beta1_grad(3) = (2*B*D - C*F) / (DEN*DEN2);beta1_grad(4) = (-C*D + 2*F) / (DEN*DEN2);beta1_grad(5) = -1 / DEN2;% Standard deviation (STD)beta1_std = sqrt(beta1_grad' * BCDFG_cov * beta1_grad);​end%% Function phiFitFunfunction [phi_fit,phi_std,phi_grad] = ...    phiFitFun(mu,nu,BCDFG,BCDFG_cov,A0_dg1,A0_dg2,A0HA0inv,B0,sig2,rho)%phiFitFun Computes the fitted value phi_fit and its uncertainty (STD),%       based on estimated ellipse coefficients BCDFG and their%       estimated covariance matrix BCDFG_cov.% (c) Viktor Witkovsky (witkovsky@savba.sk)% Ver.: 31-Jul-2014 18:27:32% Fitted coefficientsB = BCDFG(1);C = BCDFG(2);D = BCDFG(3);F = BCDFG(4);% G = BCDFG(5);% Fitted value of phase phin = length(mu);nom = sqrt(4*B - C^2)*(F + C*mu + 2*B*nu);den = (2*B*(D + 2*mu) - C*(F + C*mu));%phi_fit = atanvw(nom,den);phi_fit = atan2(nom,den);% GradientDEN = 4*B - C^2;DEN2 = sqrt(DEN)*(4*B^2*nu.^2 + B*((D + 2*mu).^2 + ...    4*(F + C*mu).*nu - C^2*nu.^2) -  ...    (F + C*mu).*(-F + C*(D + mu + C*nu)));​phi_grad = zeros(n,7);phi_grad(:,1) = (sqrt(DEN)*(-2*(F + 2*B*nu) + ...    C*(D + C*nu))) ./ (2*(4*B^2*nu.^2 + B*((D + 2*mu).^2 + ...    4*(F + C*mu).*nu - C^2*nu.^2) - (F + C*mu).*(-F + C*(D + mu + C*nu))));phi_grad(:,2) =  (2*B*sqrt(DEN)*(2*B*(D + 2*mu) - C*(F + C*mu))) ./ ...    ((-2*B*(D + 2*mu) + C*(F + C*mu)).^2 + ...    (4*B - C^2)*(F + C*mu + 2*B*nu).^2);      phi_grad(:,3) = (4*B^2*(D + 2*mu).*nu - ...    2*B*(F + C*mu).*(D + 2*mu + 3*C*nu) + C*(F + C*mu).* ...    (-F + C*(D + mu + C*nu))) ./ (2*B*DEN2);phi_grad(:,4) = (-2*C^2*mu.*(D + mu) - C^3*mu.*nu - ...    C*(D - 2*mu).*(F + 2*B*nu) + ...    2*(F^2 + 2*B*mu.*(D + 2*mu) + 2*B*F*nu)) ./ (2*DEN2);phi_grad(:,5) = (-2*B*sqrt(DEN)*(F + C*mu + 2*B*nu)) ./ ...    ((-2*B*(D + 2*mu) + C*(F + C*mu)).^2 + ...    (4*B - C^2)*(F + C*mu + 2*B*nu).^2);phi_grad(:,6) = sqrt(DEN)*(D + 2*mu + C*nu) ./ ...    (2*(4*B^2*nu.^2 + B*((D + 2*mu).^2 + 4*(F + C*mu).*nu - C^2*nu.^2) -...    (F + C*mu).*(-F + C*(D + mu + C*nu))));% Compute H*A' = [I rho*I; rho*I I] * [diag(A0_dg1); diag(A0_dg2)]HA0_dg1 = A0_dg1 + rho*A0_dg2;HA0_dg2 = A0_dg2 + rho*A0_dg1;% B1B1inv = sparse(1:n,1:n,1./(B1_diag1.^2 + B1_diag2.^2));A0grad = HA0_dg1.*phi_grad(:,1) + HA0_dg2.*phi_grad(:,2);B0A0grad =  repmat(A0HA0inv * A0grad,1,5) .* B0;% Variance (VAR) of phi_i (contributions to variance of estimator of phi_i)% phi_var = phi_grad(:,1:2)'*Sigma_11*phi_grad(:,1:2) + %           2*phi_grad(:,1:2)'*Sigma_12*phi_grad(:,3:end) + %           phi_grad(:,3:end)'*Sigma_22*phi_grad(:,3:end)phi_var = sig2*((phi_grad(:,1).^2 + phi_grad(:,2).^2) + ...      2*rho * phi_grad(:,1) .* phi_grad(:,2) - ...      A0grad .* (A0HA0inv*A0grad));phi_var = phi_var + sum((B0A0grad*BCDFG_cov) .* B0A0grad,2);phi_var = phi_var + sum((B0A0grad*BCDFG_cov) .* phi_grad(:,3:end),2);phi_var = phi_var + sum((phi_grad(:,3:end)*BCDFG_cov).*phi_grad(:,3:end),2);% Standard deviation (STD) of phi_iphi_std = sqrt(phi_var);​end%% Function munuStdFunfunction [mu_std,nu_std] = ...    munuStdFun(theta_cov,A0_dg1,A0_dg2,A0HA0_inv,B0,sigma2,rho)%munuStdFun Computes the uncertainties (STDs), of the mu_fit and nu_fit.% (c) Viktor Witkovsky (witkovsky@savba.sk)% Ver.: 31-Jul-2014 18:27:32% Compute H*A' = [I rho*I; rho*I I] * [diag(A0_dg1); diag(A0_dg2)]HA0_dg1 = A0_dg1 + rho*A0_dg2;HA0_dg2 = A0_dg2 + rho*A0_dg1;​HAAHAB1 =  repmat(A0HA0_inv * HA0_dg1,1,5) .* B0;mu_std = sqrt(sigma2*(1 - HA0_dg1 .* (A0HA0_inv*HA0_dg1)) ...    + sum((HAAHAB1*theta_cov) .* HAAHAB1,2));​HAAHAB2 =  repmat(A0HA0_inv * HA0_dg2,1,5) .* B0;nu_std = sqrt(sigma2*(1 - HA0_dg2 .* (A0HA0_inv*HA0_dg2)) ...    + sum((HAAHAB2*theta_cov) .* HAAHAB2,2));​end%% Function smoothByFFTfunction [xs,ys,options] = smoothedDataByFFT(x,y,options)% smoothByFFT Smooth the original measurements (x,y) by using the %    significant FFT frequencies% (c) Viktor Witkovsky (witkovsky@savba.sk)% Ver.: 22-Feb-2014 15:10:24​narginchk(2, 3);if nargin < 3, options = []; end​if  ~isfield(options, 'fftthreshold')    options.fftthreshold = 0.1;endif  ~isfield(options, 'fftsmooththreshold')    options.fftsmooththreshold = 0.01;endfftthreshold = options.fftthreshold;fftsmooththreshold = options.fftsmooththreshold;if  ~isfield(options, 'fftindx')    fftx = abs(fftshift(fft(x)));    fftmaxx = max(fftx);    fftindx1 = find(fftx >= fftmaxx *fftthreshold);    fftindx1 = unique(sort([fftindx1;mean(fftindx1)]));    fftindx2 = find(fftx >= fftmaxx *fftsmooththreshold);    fftindx2 = unique(sort([fftindx2;mean(fftindx2)]));    options.fftindx = fftindx2;endif  ~isfield(options, 'fftindy')    ffty = abs(fftshift(fft(y)));    fftmaxy = max(ffty);    fftindy1 = find(ffty >= fftmaxy *fftthreshold);    fftindy1 = unique(sort([fftindy1;mean(fftindy1)]));    fftindy2 = find(ffty >= fftmaxy *fftsmooththreshold);    fftindy2 = unique(sort([fftindy2;mean(fftindy2)]));    options.fftindy = fftindy2;endindx1 = fftindx1;indy1 = fftindy1;​xfft = fftshift(fft(x));indX1 = setdiff(1:numel(xfft),indx1);xfft(indX1) = 0;xs = ifft(ifftshift(xfft));​yfft = fftshift(fft(y));indY1 = setdiff(1:numel(yfft),indy1);yfft(indY1) = 0;ys = ifft(ifftshift(yfft));end

%% ellipseFit4HC_EXAMPLE​​clear​%% GENERATE the uncorrelated measurements: x and y %  Feel free to experiment with the setting parameters​alpha0true = 0;                             % x-center (offset x)beta0true  = 0;                             % y-center (offset x)alpha1true = 1;                             % x-amplitudebeta1true  = 1;                             % y-amplitudephi0true   = 0;                             % phase offset​sigma = 0.001;                          % std of x and y independent errorsn = 500;                               % number of measurements pointscycles = 1;                             % measurement interval,  # cyclesphitrue = cycles*(2*pi)*sort(rand(n,1));    % true phases phi_i​% true values:  X and YXmean = @(t) alpha0true + alpha1true * cos(t);Ymean = @(t) beta0true + beta1true * sin(t + phi0true);​% measurements:  X + error, Y + error,x = Xmean(phitrue) + sigma * randn(size(phitrue));y = Ymean(phitrue) + sigma * randn(size(phitrue));​%% Fit the ellipse based on measured data​options.alpha = 0.05;options.correlation = 0;options.displconst = 633.3/(4*pi);options.displunit = 'nanometer [nm]';result = ellipseFit4HC(x,y,options);​disp(result)​%% Display statistics of the fitted displacements (!! Table with n rows !!)​TABLE1 = result.TABLE_EllipsePars;TABLE2 = result.TABLE_Displacements;​disp(TABLE2)disp(['Displacement unit: ' result.Displacement_unit])​%% Plot the stat. uncertainty of displacement​displ_std = result.Displacement_std;​figureplot(phitrue, displ_std)xlabel('phi');ylabel(['Statistical uncertainty of displacement, ',result.Displacement_unit]);title(['Statistical Uncertainty: Min: ', ...    num2str(result.Displacement_std_min), ', Max: ', ...    num2str(result.Displacement_std_max) ]);​%% Plot the displacements with expanded uncertainties​displ_fit = result.Displacement_fit;const = result.Displacement_const;displ_true = const*phitrue;displ_res = displ_fit - displ_true;ix = (result.phi_fit-phitrue) < -1;displ_res(ix) = displ_res(ix) + 2*pi*const;​figureplot(displ_true, displ_res ,'o')hold plot(displ_true, 2*displ_std ,'r-')plot(displ_true, -2*displ_std ,'r-')gridxlabel('True displacement');ylabel('Displacement Residuals');title('Expanded Uncertainty of Fitted Displacement');​%% Plot the X residuals with the expanded uncertainties​figureplot(phitrue,result.x_res_fit,'.')holdplot(phitrue,2*result.x_res_std,'r-')plot(phitrue,-2*result.x_res_std,'r-')gridxlabel('True phi');ylabel('x residuals');title('Expanded Uncertainty of Fitted X Residuals');​​%% Plot the Y residuals with the expanded uncertainties​figureplot(phitrue,result.y_res_fit,'.')holdplot(phitrue,2*result.y_res_std,'r-')plot(phitrue,-2*result.y_res_std,'r-')gridxlabel('True phi');ylabel('y residuals');title('Expanded Uncertainty of Fitted Y Residuals');​​%% CORRELATED MEASUREMENTS​clear​%% GENERATE the correlated measurements: x and y alpha0true = 0;                             % x-center (offset x)beta0true  = 0;                             % y-center (offset x)alpha1true = 1;                             % x-amplitudebeta1true  = 0.98;                             % y-amplitudephi0true   = pi/10;                         % phase offset​sigma = 0.005;                           % std of x and y independent errorsn = 1000;                               % number of measurements pointscycles = 1;                             % measurement interval,  # cyclesphitrue = cycles*(2*pi)*sort(rand(n,1));    % true phases phi_i​% true values:  X and YXmean = @(t) alpha0true + alpha1true * cos(t);Ymean = @(t) beta0true + beta1true * sin(t + phi0true);​rho = 0.9;err = mvnrnd([0 0],sigma^2*[1 rho; rho 1],n);​% AR errors + correlated x and y% rhoAR = -.99995;% err = sigma*filter(1,[1 rhoAR],mvnrnd([0 0],[1 rho; rho 1],n));​x = Xmean(phitrue) + err(:,1);y = Ymean(phitrue) + err(:,2);​%% Fit the ellipse based on measured data​options.alpha = 0.05;options.correlation = rho; % Set the true correlation (assumed to be known)options.displconst = 633.3/(4*pi);options.displunit = 'nanometer [nm]';result = ellipseFit4HC(x,y,options);​disp(result)​%% Display statistics of the fitted displacements (!! Table with n rows !!)​TABLE1 = result.TABLE_EllipsePars;TABLE2 = result.TABLE_Displacements;​disp(TABLE2)disp(['Displacement unit: ' result.Displacement_unit])​%% Plot the stat. uncertainty of displacement​displ_std = result.Displacement_std;​figureplot(phitrue, displ_std)xlabel('phi');ylabel(['Statistical uncertainty of displacement, ',result.Displacement_unit]);title(['Statistical Uncertainty: Min: ', ...    num2str(result.Displacement_std_min), ', Max: ', ...    num2str(result.Displacement_std_max) ]);​%% Plot the displacements with expanded uncertainties​displ_fit = result.Displacement_fit;const = result.Displacement_const;displ_true = const*phitrue;displ_res = displ_fit - displ_true;ix = (result.phi_fit-phitrue) < -1;displ_res(ix) = displ_res(ix) + 2*pi*const;​figureplot(displ_true, displ_res ,'o')hold plot(displ_true, 2*displ_std ,'r-')plot(displ_true, -2*displ_std ,'r-')gridxlabel('True displacement');ylabel('Displacement Residuals');title('Expanded Uncertainty of Fitted Displacement');​%% Plot the X residuals with the expanded uncertainties​figureplot(phitrue,result.x_res_fit,'.')holdplot(phitrue,2*result.x_res_std,'r-')plot(phitrue,-2*result.x_res_std,'r-')gridxlabel('True phi');ylabel('x residuals');title('Expanded Uncertainty of Fitted X Residuals');​​%% Plot the Y residuals with the expanded uncertainties​figureplot(phitrue,result.y_res_fit,'.')holdplot(phitrue,2*result.y_res_std,'r-')plot(phitrue,-2*result.y_res_std,'r-')gridxlabel('True phi');ylabel('y residuals');title('Expanded Uncertainty of Fitted Y Residuals');​

3 仿真结果

4 参考文献

[1]吴美容, and 王建国. "Matlab在离散点拟合椭圆及极值距离计算中的应用." 地矿测绘 32.4(2016):4.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。