椭圆方程拟合（最小二乘，matlab源代码）

1.椭圆方程定义

椭圆一般方程为
$$a{x}^2+bxy+c{y}^2+dx+ey+1=0.$$

2.最小能量函数定义

要使得圆方程最为准确，则要是所有的点尽可能满足方程，因此需使函数F
$$F=\sum (a{x_i}^2+b{x}_i{y}_i+c{y_i}^2+d{x}_i+e{y}_i{)}^2,i\in [1,N],N为点数目$$

但实际上其不可能为0，因此根据最小二乘法求解其最小值
对函数F求导可得
$$\frac{\partial F}{a}=\sum 2(a{x_i}^2+b{x}_i{y}_i+c{y_i}^2+d{x}_i+e{y}_i+1)\ast {x_i}^2$$

$$\frac{\partial F}{b}=\sum 2(a{x_i}^2+b{x}_i{y}_i+c{y_i}^2+d{x}_i+e{y}_i+1)\ast x_i{y}_i$$

$$\frac{\partial F}{c}=\sum 2(a{x_i}^2+b{x}_i{y}_i+c{y_i}^2+d{x}_i+e{y}_i+1)\ast {y_i}^2$$

$$\frac{\partial F}{d}=\sum 2(a{x_i}^2+b{x}_i{y}_i+c{y_i}^2+d{x}_i+e{y}_i+1)\ast x_i$$

$$\frac{\partial F}{e}=\sum 2(a{x_i}^2+b{x}_i{y}_i+c{y_i}^2+d{x}_i+e{y}_i+1)\ast y_i$$

当这些导数为0是，函数最小
为方便

求解，将其写成a,b,c的矩阵函数形式，则可得
$$\left[\begin{array}{ccccc}\sum ({x_i}^4)& \sum ({x_i}^3\ast y_i)& \sum ({x_i}^2\ast {y_i}^2)& \sum ({x_i}^3)& \sum (y_i\ast {x_i}^2)\\ \sum ({x_i}^3\ast y_i)& \sum ({x_i}^2\ast {y_i}^2)& \sum (x_i\ast {y_i}^3)& \sum ({x_i}^2\ast y_i)& \sum (x_i\ast {y_i}^2)\\ \sum ({x_i}^2\ast {y_i}^2)& \sum (x_i\ast {y_i}^3)& \sum ({y_i}^4)& \sum (x_i\ast {y_i}^2)& \sum ({y_i}^3)\\ \sum ({x_i}^3)& \sum ({x_i}^2\ast y_i)& \sum (x_i\ast {y_i}^2)& \sum ({x_i}^2)& \sum (x_i\ast y_i)\\ \sum (y_i\ast {x_i}^2)& \sum (x_i\ast {y_i}^2)& \sum ({y_i}^3)& \sum (x_i\ast y_i)& \sum ({y_i}^2)\end{array}\right]\left[\begin{array}{c}a\\ b\\ c\\ d\\ e\end{array}\right]=\left[\begin{array}{c}-\sum ({x_i}^2)\\ -\sum (x_i\ast y_i)\\ -\sum ({y_i}^2)\\ -\sum (x_i)\\ -\sum (y_i)\end{array}\right]$$

可变形为

$$AX=B$$
接下来便是求解方程AX=B

3.matlab代码

1. 实例

% 圆拟合实例
clc;
close all;

%% 创建测试数据
num =1000;
% 噪声
rng(1);
noise1=0.2*randn(num,1);
rng(2);
noise2=0.2*randn(num,1);
% 创建初始数据
center = [10,5];
axea =3;
axeb=4;
angle= 0.2;
s = sin(angle); c = cos(angle);
Q =[c -s; s c]
theta=(2*rand(num,1)-1)*pi;
xx = axea*cos(theta);
yy = axeb*sin(theta);
x = Q(1,:)*[xx yy]' + center(1,1)+noise1';
y = Q(2,:)*[xx yy]' + center(1,2)+noise2';
x = x';
y = y';

%% 拟合椭圆
figure;
scatter(x,y);
hold on;
[a,b,c,d,e] = fit_elipse(x,y);
[centrex, centrey, axea, axeb, angle] = convertConicToParametric([a,b,c,d,e,1]);%获取的长短轴先短后长，可能发生调换
DrawConic([a,b,c,d,e,1],'b',1,1);



function [contour,center,ellipse_para] = DrawConic(C,color,lineWidth,scale)
%DRAWELLIPSE	Draw ellipse
%
%	drawellipse (z, a, b, alpha, pat{'-'}, OPTIONS{[]})
%	draws ellipse into current figure.
%
%	z, a, b, alpha: parameters of ellipse
%	pat: pattern to be used

mc = max(size(C));
if mc==6
    %equation implicite
    [centrex, centrey, axea, axeb, angle] = convertConicToParametric(C);
    ellipse_para=[centrex, centrey, axea, axeb, angle];
elseif mc==3
    par = debuildConic(C);
    [centrex, centrey, axea, axeb, angle] = convertConicToParametric(par);
elseif mc==5
    centrex = C(1);
    centrey = C(2);
    axea = C(3);
    axeb = C(4);
    angle = C(5);
else
    disp('Error in draw conic, format parametres non conforme')
    return
end
axea =scale*axea;
axeb=scale*axeb;
s = sin(angle); c = cos(angle);
Q =[c -s; s c];
theta = [0:0.02:2*pi]';
xx = axea*cos(theta);
yy = axeb*sin(theta);

x = Q(1,:)*[xx yy]' + centrex;
y = Q(2,:)*[xx yy]' + centrey;
contour=[x;y]';
plot(x,y, '-','Color',color,'LineWidth',lineWidth);

x1 = [centrex-5,centrex,centrex+5];
plot(x1,[centrey,centrey,centrey], '-','Color',color,'LineWidth',lineWidth);
y1 = [centrey-5,centrey,centrey+5];
plot([centrex,centrex,centrex],y1, '-','Color',color,'LineWidth',lineWidth);
center =[centrex,centrey];
end


function [centrex, centrey, axea, axeb, angle] = convertConicToParametric(par)
%this function will convert parameters from the form
%Ax^2 + Bxy + Cy^2 + Dx + Ey + F = 0   
%to
%centrex centrey axea axeb angle
%
%take as input :
%par = [A B C D E F]


%assure forme compatible
if size(par,1)==3
    par = debuildConic(par);
end

% Convert to geometric radii, and centers

thetarad = 0.5*atan2(par(2),par(1) - par(3));
cost = cos(thetarad);
sint = sin(thetarad);
sin_squared = sint.*sint;
cos_squared = cost.*cost;
cos_sin = sint .* cost;

Ao = par(6);
Au =   par(4) .* cost + par(5) .* sint;
Av = - par(4) .* sint + par(5) .* cost;
Auu = par(1) .* cos_squared + par(3) .* sin_squared + par(2) .* cos_sin;
Avv = par(1) .* sin_squared + par(3) .* cos_squared - par(2) .* cos_sin;


if(Auu==0 | Avv==0)
    %problem.  this is not a valid ellipse
    %make sure param are invalid and be easy to spot
    centrex = -1;
    centrey = -1;
    axea = 0;
    axeb = 0;
    angle = 0;
    return;   
end


% ROTATED = [Ao Au Av Auu Avv]

tuCentre = - Au./(2.*Auu);
tvCentre = - Av./(2.*Avv);
wCentre = Ao - Auu.*tuCentre.*tuCentre - Avv.*tvCentre.*tvCentre;

uCentre = tuCentre .* cost - tvCentre .* sint;
vCentre = tuCentre .* sint + tvCentre .* cost;

Ru = -wCentre./Auu;
Rv = -wCentre./Avv;

Ru = sqrt(abs(Ru)).*sign(Ru);
Rv = sqrt(abs(Rv)).*sign(Rv);


centrex = uCentre;
centrey = vCentre;
axea = Ru;
axeb = Rv;
angle = thetarad;
end

2. 椭圆拟合函数

function [a,b,c,d,e] =  fit_elipse(x,y)
% function fit_circle.m
% -------------------------------------------------------------------------
% 椭圆拟合
% 椭圆方程按照ax^2+bxy+cy^2+dx+ey+1=0
% 最小化函数定义为 sum(a(x_i)^2+b(x_i)(y_i)+c(y_i)^2+d(x_i)+e(y_i)+1),i为1到n的整数
% 计算导数，化为AX=B的形式，X为[a;b;c;d;e]
% Inputs:
%       x = 点x坐标,写成(1000,1)竖排格式
%       y = 点y坐标
% Output:
%       a,b,c,d,e = 椭圆方程对应参数
% Last updated: November 14, 2022

% 数据预处理
xx = x.*x;
xxx = x.^3;
yy= y.*y;
yyy = y.^3;
xy = x.*y;

% 构建A
A1_1 = sum(x.^4);
A1_2 = sum(xxx.*y);
A1_3 = sum(xx.*yy);
A1_4 = sum(xxx);
A1_5 = sum(y.*xx);

A2_1 = A1_2;
A2_2 = sum(xx.*yy);
A2_3 = sum(x.*yyy);
A2_4 = sum(xx.*y);
A2_5 = sum(x.*yy);


A3_1 = A1_3;
A3_2 = A2_3;
A3_3 = sum(yy.*yy) ;
A3_4 = sum(x.*yy) ;
A3_5 = sum(yy.*y) ;

A4_4 = sum(xx);
A4_5 = sum(xy);
A5_5 = sum(yy);

A=[A1_1,A1_2,A1_3,A1_4,A1_5;
   A2_1,A2_2,A2_3,A2_4,A2_5;
   A3_1,A3_2,A3_3,A3_4,A3_5;
   A1_4,A2_4,A3_4,A4_4,A4_5;
   A1_5,A2_5,A3_5,A4_5,A5_5];

% 构建B
B1 = -sum(xx);
B2 = -sum(xy);
B3 = -sum(yy);
B4 = -sum(x);
B5 = -sum(y);

B = [B1;B2;B3;B4;B5];

% 计算
X = A\B;
a=X(1);
b=X(2);
c=X(3);
d=X(4);
e=X(5);

end