【Matlab&Mathematica】對三維空間上的點進行橢圓擬合
問題是這樣：比如有一個地心慣性系的軌道，然後從軌道上取了幾個點，問能不能根據這幾個點把軌道還原了？
當然，如果知道軌道這幾個點的速度的情況下，根據軌道六根數也是能計算軌道的，不過真近點角是隨時間變動的。
下面我會用數學的方法來解這個問題，基本思想是通過擬合空間上點的平面與橢球平面的交線將該軌道計算出來，算是一種思路吧。
首先需要有軌道資料，我們就從STK上獲得，我使用預設引數生成了一個軌道，如下圖：
輸出j2000下的位置速度：
取其中5個點進行擬合：
可以先計算橢球，設橢球方程為x^2/a+y^2/b+z^2/c+d=0，然後求其最小二乘函式f(a,b,c,d) = sum((x^2/a+y^2/b+z^2/c+d)^2)。
通過單純性法求該函式符合上面5個點的最小值時的a,b,c,d四個引數。
matlab裡就是用fminsearch函式就行了。
程式碼如下：
clear all; close all; clc; x=[4272.199656,8091.936548,9250.537919,7326.513290,4775.406342]; y=[-9122.847094,-8215.105235,-4377.145579,3801.632308,7889.177050]; z=[332.461913,8019.448934,13254.361230,16413.922920,15098.049929];[email protected](t)sum((x.^2/t(1)+y.^2/t(2)+z.^2/t(3)+t(4)).^2); [t fval]= fminsearch( fun , rand(4,1)) ; %單純形法多元優化
t
這裡的t就是要求的a,b,c,d四個引數。
我求的引數是
11831.9652077381
7748.53668377191
37674.4028071175
-14504.0838741735
得到橢球方程為：
x^2/11831.96520773810 + y^2/7748.536683771910 + z^2/37674.40280711745 - 14504.08387417353 = 0
Mathematica中畫出來就是：
ContourPlot3D[
x^2/11831.96520773810 + y^2/7748.536683771910 +
z^2/37674.40280711745 - 14504.08387417353 == 0, {x, -20000,
20000}, {y, -20000, 20000}, {z, -30000, 30000}]
橢球有了，下面我們求平面，使用ransac進行平面擬合。
我參考了這篇部落格：https://blog.csdn.net/u010128736/article/details/53422070
不過他好像也參考了我過去寫的ransac直線擬合：https://www.cnblogs.com/tiandsp/archive/2013/06/03/3115743.html，有趣 : )
程式碼如下：
clc;clear all;close all;
iter = 1000;
x=[4272.199656,8091.936548,9250.537919,7326.513290,4775.406342];
y=[-9122.847094,-8215.105235,-4377.145579,3801.632308,7889.177050];
z=[332.461913,8019.448934,13254.361230,16413.922920,15098.049929];
data=[x;y;z];
%%% 繪製資料點
figure;plot3(data(1,:),data(2,:),data(3,:),'o');hold on; % 顯示資料點
number = size(data,2); % 總點數
bestParameter1=0; bestParameter2=0; bestParameter3=0; % 最佳匹配的引數
sigma = 1;
pretotal=0; %符合擬合模型的資料的個數
for i=1:iter
%%% 隨機選擇三個點
idx = randperm(number,3);
sample = data(:,idx);
%%%擬合直線方程 z=ax+by+c
plane = zeros(1,3);
x = sample(1, :);
y = sample(2, :);
z = sample(3, :);
a = ((z(1)-z(2))*(y(1)-y(3)) - (z(1)-z(3))*(y(1)-y(2)))/((x(1)-x(2))*(y(1)-y(3)) - (x(1)-x(3))*(y(1)-y(2)));
b = ((z(1) - z(3)) - a * (x(1) - x(3)))/(y(1)-y(3));
c = z(1) - a * x(1) - b * y(1);
plane = [a b -1 c]
mask=abs(plane*[data; ones(1,size(data,2))]); %求每個資料到擬合平面的距離
total=sum(mask<sigma); %計算資料距離平面小於一定閾值的資料的個數
if total>pretotal %找到符合擬合平面資料最多的擬合平面
pretotal=total;
bestplane=plane; %找到最好的擬合平面
end
end
%顯示符合最佳擬合的資料
mask=abs(bestplane*[data; ones(1,size(data,2))])<sigma;
hold on;
k = 1;
for i=1:length(mask)
if mask(i)
inliers(1,k) = data(1,i);
inliers(2,k) = data(2,i);
plot3(data(1,i),data(2,i),data(3,i),'r+');
k = k+1;
end
end
%%% 繪製最佳匹配平面
bestParameter1 = bestplane(1);
bestParameter2 = bestplane(2);
bestParameter3 = bestplane(4);
xAxis = min(inliers(1,:)):max(inliers(1,:));
yAxis = min(inliers(2,:)):max(inliers(2,:));
[x,y] = meshgrid(-30000:1000:30000);
z = bestParameter1 * x + bestParameter2 * y + bestParameter3;
mesh(x, y, z);
title(['bestPlane: z = ',num2str(bestParameter1),'x + ',num2str(bestParameter2),'y + ',num2str(bestParameter3)]);
擬合結果如下：
然後我們在Mathematica上畫出平面與橢球：
程式碼如下：
data = {{4272.199656, -9122.847094,
332.461913}, {8091.936548, -8215.105235,
8019.448934}, {9250.537919, -4377.145579,
13254.361230}, {7326.513290, 3801.632308, 16413.922920},
{4775.406342, 7889.177050 , 15098.049929}};
P3 = ListPlot3D[data, ColorFunction -> Function[{x, y, z}, Hue[x]]]
P2 = ContourPlot3D[
1.8204 x + 0.81444 y - z - 20.3705 == 0, {x, -30000,
30000}, {y, -30000, 30000}, {z, -30000, 30000}]
P1 = ContourPlot3D[
x^2/11831.96520773810 + y^2/7748.536683771910 +
z^2/37674.40280711745 - 14504.08387417353 == 0, {x, -20000,
20000}, {y, -20000, 20000}, {z, -30000, 30000}]
Show[{P1, P2, P3}]
結果如下：
感覺有那麼點意思了。
下面我們求出兩個面相交的引數方程，使用Mathematica計算還是比較方便的。
列出以下三個方程：
1.8204 x + 0.81444 y - z - 20.3705 = 0
x^2/11831.96520773810 + y^2/7748.536683771910 + z^2/37674.40280711745 - 14504.08387417353 = 0
z = 23375.89994468299 Sin[t]
這裡23375.89994468299 通過sqrt(14504.08387417353*37674.40280711745)求出來的，z是在這個範圍內變動的。
使用Mathematica解算如下：
Solve[{1.8204 x + 0.81444 y - z - 20.3705 == 0,
x^2/11831.96520773810 + y^2/7748.536683771910 +
z^2/37674.40280711745 - 14504.08387417353 == 0,
z == 23375.89994468299 Sin[t]}, {x, y, z}]
結果：
{{x -> 2.5336*10^-43 (3.90483*10^43 + 4.48093*10^46 Sin[t] - 7.40177*10^18 Sqrt[
5.65524*10^54 - 8.37291*10^51 Sin[t] - 1.04594*10^55 Sin[t]^2]),
y -> 1.2668*10^-42 (2.28816*10^42 + 2.62575*10^45 Sin[t] + 3.30882*10^18 Sqrt[
5.65524*10^54 - 8.37291*10^51 Sin[t] - 1.04594*10^55 Sin[t]^2]),
z -> 23375.9 Sin[t]},
{x -> 2.5336*10^-43 (3.90483*10^43 + 4.48093*10^46 Sin[t] + 7.40177*10^18 Sqrt[
5.65524*10^54 - 8.37291*10^51 Sin[t] - 1.04594*10^55 Sin[t]^2]),
y -> 1.2668*10^-42 (2.28816*10^42 + 2.62575*10^45 Sin[t] - 3.30882*10^18 Sqrt[
5.65524*10^54 - 8.37291*10^51 Sin[t] - 1.04594*10^55 Sin[t]^2]),
z -> 23375.9 Sin[t]}}
求到這裡，這個引數方程已經求出來的，下面我畫出來驗證一下。
matlab程式碼如下：
clear all;
close all;
clc;
xx=[];yy=[];zz=[];
for t=0:0.001:2*pi
x=2.5336*10^(-43) *(3.90483*10^43 + 4.48093*10^46*sin(t) - 7.40177*10^18*sqrt(5.65524*10^54 - 8.37291*10^51* sin(t) - 1.04594*10^55*sin(t).^2));
y=1.2668*10^(-42) *(2.28816*10^42 + 2.62575*10^45*sin(t) + 3.30882*10^18*sqrt(5.65524*10^54 - 8.37291*10^51 *sin(t) - 1.04594*10^55* sin(t).^2));
z=23375.9* sin(t);
if(sqrt(5.65524*10^54 - 8.37291*10^51 *sin(t) - 1.04594*10^55* sin(t).^2)>0)
xx=[xx x];
yy=[yy y];
zz=[zz z];
end
end
for t=0:0.001:2*pi
x=2.5336*10^(-43) *(3.90483*10^43 + 4.48093*10^46*sin(t) + 7.40177*10^18*sqrt(5.65524*10^54 - 8.37291*10^51* sin(t) - 1.04594*10^55*sin(t).^2));
y=1.2668*10^(-42) *(2.28816*10^42 + 2.62575*10^45*sin(t) - 3.30882*10^18*sqrt(5.65524*10^54 - 8.37291*10^51 *sin(t) - 1.04594*10^55* sin(t).^2));
z=23375.9* sin(t);
if(sqrt(5.65524*10^54 - 8.37291*10^51 *sin(t) - 1.04594*10^55* sin(t).^2)>0)
xx=[xx x];
yy=[yy y];
zz=[zz z];
end
end
plot3(xx,yy,zz,'.')
x=[4272.199656,8091.936548,9250.537919,7326.513290,4775.406342];
y=[-9122.847094,-8215.105235,-4377.145579,3801.632308,7889.177050];
z=[332.461913,8019.448934,13254.361230,16413.922920,15098.049929];
hold on
plot3(x,y,z,'ro')
結果如下：
可以看出，點基本都在橢圓周圍，效果不錯，下面多用幾組原始點對比看看：
呵呵，這就比較尷尬了，好像不怎麼一樣哦。
畢竟，這裡只用了5個點，擬合點數多一些效果應該會好些吧 : )
關注公眾號： MATLAB基於模型的設計 （ID：xaxymaker） ，每天推送MATLAB學習最常見的問題，每天進步一點點，業精於勤荒於嬉。
開啟微信掃一掃哦！