可操作楕円体の計算

ヤコビアンJのとき、
JJ^Tの固有ベクトルが長軸、短軸の方向を表し、
JJ^Tの固有値が長軸、短軸の大きさを表す。

楕円の関係式
勉強しよう数学解答集: 原点の回りに回転した楕円の方程式から楕円の軸を計算する

clear; close all;

syms L1 L2 L3;
syms th1 th2 th3;
x1 = L1*cos(th1);
y1 = L1*sin(th1);
x2 = L1*cos(th1)+L2*cos(th1+th2);
y2 = L1*sin(th1)+L2*sin(th1+th2);
x3 = L1*cos(th1)+L2*cos(th1+th2)+L3*cos(th1+th2+th3);
y3 = L1*sin(th1)+L2*sin(th1+th2)+L3*sin(th1+th2+th3);

J1 = [diff(x1, th1);
    diff(y1, th1)];
J2 = [diff(x2, th1) diff(x2, th2);
    diff(y2, th1) diff(y2, th2)];
J3 = [diff(x3, th1) diff(x3, th2) diff(x3, th3);
    diff(y3, th1) diff(y3, th2) diff(y3, th3)];
l1 = 1;
l2 = 0.6;
l3 = 0.8;
%destination (px,py)=(2,1.5);
px = 1.8;
py = 1.5;
%px = 0.9;
%py = 1.2;

W = eye(2);%x, yに対する重み行列

t1 = 0;
t2 = 0;
t3 = 0;
epsilon = 0.1;
e = 1e10;% big number (placeholder)
while norm(e) > 0.1 %誤差ベクトルのノルムで評価
    px3_tmp = vpa(subs(x3, ...
        [L1 L2 L3 th1 th2 th3], ...
        [l1 l2 l3 t1 t2 t3]));
    py3_tmp = vpa(subs(y3, ...
        [L1 L2 L3 th1 th2 th3], ...
        [l1 l2 l3 t1 t2 t3]));
    J3_tmp = vpa(subs(J3,...
        [L1 L2 L3 th1 th2 th3],...
        [l1 l2 l3 t1 t2 t3]));
    ex = px - px3_tmp;
    ey = py - py3_tmp;
    e = [ex;ey]; %誤差ベクトル
    dq = inv(transpose(J3_tmp)*W*J3_tmp+epsilon*eye(3))*...
        transpose(J3_tmp)*W*e;
    t1 = t1 + dq(1);
    t2 = t2 + dq(2);
    t3 = t3 + dq(3);
    norm(e)
end

x1i = vpa(subs(x1, ...
    [L1 th1], ...
    [l1 t1]));
y1i = vpa(subs(y1, ...
    [L1 th1], ...
    [l1 t1]));
x2i = vpa(subs(x2, ...
    [L1 L2 th1 th2], ...
    [l1 l2 t1 t2]));
y2i = vpa(subs(y2, ...
    [L1 L2 th1 th2], ...
    [l1 l2 t1 t2]));
x3i = vpa(subs(x3, ...
    [L1 L2 L3 th1 th2 th3], ...
    [l1 l2 l3 t1 t2 t3]));
y3i = vpa(subs(y3, ...
    [L1 L2 L3 th1 th2 th3], ...
    [l1 l2 l3 t1 t2 t3]));

J3_square = J3_tmp*transpose(J3_tmp);
[V,D] = eig(J3_square);
v1 = V(:,1);
v2 = V(:,2);
eigs = eig(J3_square);
e1 = eigs(1);
e2 = eigs(2);

syms A B C;
syms x y;
ellipse = A*x^2+B*x*y+C*y^2;
%楕円状の点（３点）
pex1 = px+abs(e1)*v1(1);
pey1 = py+abs(e1)*v1(2);
pex2 = px+abs(e2)*v2(1);
pey2 = py+abs(e2)*v2(2);
pex3 = px-abs(e1)*v1(1);
pey3 = py-abs(e1)*v1(2);
%楕円の方程式（３本）
eq1 = subs(ellipse, [x y], [pex1,pey1])==1;
eq2 = subs(ellipse, [x y], [pex2,pey2])==1;
eq3 = subs(ellipse, [x y], [pex3,pey3])==1;
% assume(A>0);
% assume(C>0);
% assume(A*C-B^2>0);
%線形方程式の求解
[Mat,vec] = equationsToMatrix([eq1, eq2, eq3], [A, B, C]);
X = linsolve(Mat,vec);
A = double(X(1));
B = double(X(2));
C = double(X(3));
theta = 1/2*atan(B/(A-C));
R = [cos(theta) -sin(theta);
     sin(theta)   cos(theta)];
cla;
hold on;

plot([0 x1i],[0 y1i],'k-');
plot([x1i x2i],[y1i y2i],'k-');
plot([x2i x3i],[y2i y3i],'k-');
plot(0,0,'ko','LineWidth',3);
plot(x1i,y1i,'bo','LineWidth',3);
plot(x2i,y2i,'go','LineWidth',3);
plot(x3i,y3i,'ro','LineWidth',3);

xlim([-1,3]);
ylim([-1,3]);
title(sprintf('destination x=%.2f,y=%.2f',px,py));
big;

scale = 0.3;
for t = linspace(0,2*pi,50)
    original = [scale*abs(e2)*(cos(t));
               scale*abs(e1)*(sin(t))];
    transformed = R*original;
    tx = px+transformed(1);
    ty = py+transformed(2);
    plot(tx,ty,'bx','LineWidth',1);
end