メモ

不慮の事態に備えて，大切なデータはネットに残しておこう。ということでさっき見つけた関数をメモ。

function [func] = input_poly_2d(g0,h0,th0,ph0)
  hh1 = h0 * cos(th0);
  hh2 = h0 * sin(th0)^2;
  g1 = g0 * cos(ph0);
  g2 = g0 * sin(ph0);
  function [f,g,h] = func(s)
    x = s(1); y = s(2); z = s(3);
    hh = - (1-z^2) * hh1 + z * hh2;
    f = 0;
    g = -(x * g1 + y * g2) + hh / 4;
    % k / 2 :phi = +-pi / 2 の時のため
    h = hh * y + g / 100;
    % g / 20 :theta = 0,pi の時のため
  end
  func = @func;
end

一次は有理式じゃないと無理かな，と思っていたんですが，最終的には多項式入力で安定化できてよかったです。

追記

紆余曲折してる。試行錯誤って大変だ。

function [func] = input_poly_2d(g0, h0, th0, ph0)
  z0 = cos(th0);
  h1 = 0.01 * cos(ph0);
  %h2 = cos(th0);
  %h3 = sin(th0)^2;
  g1 = g0 * cos(ph0);
  g2 = g0 * sin(ph0);
  function [f, g, h] = func(s)
    x = s(1); y = s(2); z = s(3);
    
    %hc = z - z0;
    %hs = - (1-z^2) * h2 + z * h3;
    %hh = h0 * (hc + hs);
    hh = h0 * (z - z0) * (z * z0 + 2);
    % hc: theta - theta_0 が大きいとき
    % hs: theta - theta_0 が小さいとき
    % hc はいらないかも
    
    gg = -(x * g1 + y * g2); 
    
    f = 0;
    g = gg + hh / 10;
    % hh / 10 : phi = +-pi / 2 の時のため
    h = hh * (y + h1);
    % (y + h1): sin theta = 0 の時のため
  end
  func = @func;
end

追記

最終的にはこんな感じになった

function [func] = input_poly_2d(g0, h0, th0, ph0)
  z0 = cos(th0);
  y0 = -sin(th0)*cos(ph0);
  %h2 = cos(th0);
  %h3 = sin(th0)^2;
  g1 = g0 * cos(ph0);
  g2 = g0 * sin(ph0);
  function [f, g, h] = func(s)
    x = s(1); y = s(2); z = s(3);
    f = 0;
    s = 1;
    if(y < 0) s = -1; end
    h = h0 * (z - z0) * (z * z0 + 2) * s;
    g = -(x * g1 + y * g2) + 0.001*g0*(y - y0);
    % 0.1h: sin(phi-phi0) = 0かつsin(phi) = 0の時
  end
  func = @func;
end