圆的拟合(带拉格朗日乘子)

圆弧特征如图1所示。圆弧的特征向量为R: [φ_R, ρ_R, r]^T，其中φ_R为圆心极角，φ_R ~ N(μ_φR,σ_φR²)；ρ为圆心极径，ρ_R ~ N(μ_ρR,σ_ρR²)；为圆半径，r~N(μ_r,σ_r²)。圆弧极坐标公式为：

(1)

图1圆弧

将圆的极坐标方程(1)整理得：

(2)

其中，i∈[m,n]。上式可以化简为最小二乘形式：

(3)

其中，x = [x_i, y_i]^T，a = 1，b = [-2ρ_Rcosφ_R, -2ρ_Rsinφ_R]^T，c=ρ_R²-r²。将上式整理为矩阵形式：

(4)

其中，H=[1,b(1,1), b(2,1),c]^T，, 。由于b²(1,1)+ b²(2,1)-4c=4r²，引入拉格朗日乘子λ，得：

(5)

其中，。满足H^TCH=4r²。令得：

(6)

其中，Q为正定矩阵，待求特征向量H一定对应绝对值最小特征值λ。圆弧特征θ=arctan(H(2,1)/H(1,1)), ，。