グルグル回る軸の角度変動の平均値を出す方法#5

・グルグル回る軸の角度変動の平均値を出す方法#4:2017年02月26日

これの補足をちょいと。
計算方法を言葉で言うと
　・入力角度からsinとcosでX,Yの値を得る
　・XとYの値を移動平均
　・平均値からatan2(Y,X)で角度の平均値を得る
という処理で、角度の平均値を算出しています。

ここで問題なのがX=Y=0の時のatan2。　計算不能です。

例えばこんな時にこの異常が出現します。
角度0度が続いていて、突然角度180度に変わった時。
　　sin(0)　= 0　cos(0)　= 1
　　sin(180)= 0　cos(180)=-1
です。

sinで算出するYは、平均しても0のまま。
0度も180度もY=0です。
ところが、cosで出すXの平均値は、+1から減少しはじめ
0を通過して-1に変わります。

atan2への引数を見ると、Yはずっと0を維持しているんで
Xの値がプラスの領域は、その値がいくらであっても必ず
0度になります。
そして、Xがマイナスなら値によらず必ず180度になってし
まいます。
　　※その途中でatan2(0,0)の異常点を通過するかも！

この結果、入力角度が急激に180度回転した場合は、角度の
平均値が得られないのです。

以下は平均回数20回で、入力角度0度→180度を試してみた時の
変化です。
#6のタイミングで入力角度を変化させます。
　　degが入力角度、
　　smzが平均処理した出力角度、
　　XとYはdegに対するcos、sinの値です。

#　　 deg　smz　X　　 Y
#1　　　0　　0　1.000　0.000　入力角度、最初は0度
#2　　　0　　0　1.000　0.000
#3　　　0　　0　1.000　0.000
#4　　　0　　0　1.000　0.000
#5　　　0　　0　1.000　0.000
#6　　180　　0　0.900　0.000　入力角度を180度に変化
#7　　180　　0　0.800　0.000
#8　　180　　0　0.700　0.000　平均処理でXが減少し始める
#9　　180　　0　0.600　0.000
#10　 180　　0　0.500　0.000　しかし、Y=0なので出力角度は0度
#11　 180　　0　0.400　0.000
#12　 180　　0　0.300　0.000
#13　 180　　0　0.200　0.000
#14　 180　　0　0.100　0.000
#15　 180　　0　0.000　0.000　★atan2(0,0)でエラー発生
#16　 180　180 -0.100　0.000　Xがマイナスになると結果が180度に
#17　 180　180 -0.200　0.000
#18　 180　180 -0.300　0.000　Xはゆるやかに変化しているが、
#19　 180　180 -0.400　0.000　結果は平均処理されない
#20　 180　180 -0.500　0.000
#21　 180　180 -0.600　0.000
#22　 180　180 -0.700　0.000
#23　 180　180 -0.800　0.000
#24　 180　180 -0.900　0.000
#25　 180　180 -1.000　0.000　平均処理おわり
#26　 180　180 -1.000　0.000
#27　 180　180 -1.000　0.000

0度→180度だけでなく、180度反対側に行くのがダメなんです。
これは30度→210度の場合。

#　　 deg　smz　X　　　Y
#1　　 30　 30　0.866　0.500　最初は30度
#2　　 30　 30　0.866　0.500　sin(30)は1÷2で0.5
#3　　 30　 30　0.866　0.500　cos(30)は√3÷2で0.866
#4　　 30　 30　0.866　0.500
#5　　 30　 30　0.866　0.500
#6　　210　 30　0.779　0.450　入力角度、30度→210度に変化
#7　　210　 30　0.693　0.400　X,Yに対する移動平均処理が始まる
#8　　210　 30　0.606　0.350
#9　　210　 30　0.520　0.300
#10　 210　 30　0.433　0.250　X,Yの変化は同比率なので出力角度は同じ
#11　 210　 30　0.346　0.200
#12　 210　 30　0.260　0.150
#13　 210　 30　0.173　0.100
#14　 210　 30　0.087　0.050
#15　 210　 30　0.000　0.000　★atan2(0,0)でエラー発生
#16　 210　210 -0.087 -0.050　符号が変わったので30度の対向位置210度に
#17　 210　210 -0.173 -0.100　急激に出力角度が変化
#18　 210　210 -0.260 -0.150
#19　 210　210 -0.346 -0.200　角度の移動平均処理ができていない
#20　 210　210 -0.433 -0.250
#21　 210　210 -0.520 -0.300
#22　 210　210 -0.606 -0.350
#23　 210　210 -0.693 -0.400
#24　 210　210 -0.779 -0.450
#25　 210　210 -0.866 -0.500
#26　 210　210 -0.866 -0.500

次は30度→200度の場合。 170度の変化です。
これは180度対向じゃないので、移動平均がうまく進みます。

#　　 deg　smz　X　　　Y
#1　　 30　 30　0.866　0.500　入力角度、最初は30度
#2　　 30　 30　0.866　0.500　sin(30)は0.5
#3　　 30　 30　0.866　0.500　cos(30)は√3÷2で0.866
#4　　 30　 30　0.866　0.500
#5　　 30　 30　0.866　0.500
#6　　200　 31　0.776　0.458　入力角度が30度→200度に変化
#7　　200　 31　0.685　0.416　移動平均処理が始まる
#8　　200　 32　0.595　0.374
#9　　200　 33　0.505　0.332　X,Yの比率が平均により変化するので
#10　 200　 35　0.415　0.289　atan2による角度はなめらかに変化
#11　 200　 37　0.324　0.247
#12　 200　 41　0.234　0.205
#13　 200　 49　0.144　0.163
#14　 200　 66　0.053　0.121
#15　 200　115 -0.037　0.079　Xの符号が変わる
#16　 200　164 -0.127　0.037
#17　 200　181 -0.217 -0.005　Yの符号も変わり出力角度が180度を超える
#18　 200　189 -0.308 -0.047
#19　 200　193 -0.398 -0.089
#20　 200　195 -0.488 -0.132
#21　 200　197 -0.579 -0.174
#22　 200　198 -0.669 -0.216
#23　 200　199 -0.759 -0.258
#24　 200　199 -0.849 -0.300
#25　 200　200 -0.940 -0.342　平均回数である20ステップかけて、
#26　 200　200 -0.940 -0.342　なめらかに30度→200度まで変化
#27　 200　200 -0.940 -0.342

「いきなり180度」、これの対策です。

まずatan2(0,0)の処理。
こんな関数にしました。　（入出力はfloatで）

/*****　ATAN2(0,0)を回避 *****/
//　　　0,0なら前回値を用いる
float atan2z(float y, float x)
{
static float d = 0.0;　　　　　　　 // 前回値
　　if((x != 0.0) || (y != 0.0)){　 // どちらか0でない
　　　　d = atan2(y, x);　　　　　　// 計算可能
　　}
　　return d;
}

X,Yが両方ゼロなら、計算可能だった直前の値を返します。
移動平均処理で何度もこの関数を通るだろうからという考えです。
また、スムージング回数を偶数ではなく奇数にすることでも、
atan2(0,0)に出くわす確率を下げられます。
　　※スムージング加算値を割り切れにくくするということで

もう一つがいきなり180度対向時の平均処理です。
これは、前回との角度差が一定値を超えた時、例えばその角度差の
半分のポイントの角度を使って平均処理を行い、「いきなり180度」
を回避するのです。
「いきなり180度」さえ回避できれば、移動平均はスムーズに進みます。

以下は、270度→90度への「いきなり180度」変化の時、変化直前に「0度」
のデータを入れてみた結果です。

#　　 deg　smz　X　　　Y
#1　　270　270　0.000 -1.000　入力角度、最初は270度
#2　　270　270　0.000 -1.000
#3　　270　270　0.000 -1.000
#4　　270　270　0.000 -1.000
#5　　270　270　0.000 -1.000　180度対向位置である90度に変化する直前
#6　　　0　273　0.050 -0.950　←中間値である「0度」を挿入
#7　　 90　273　0.050 -0.850　
#8　　 90　274　0.050 -0.750　0度を含めての移動平均処理が始まる
#9　　 90　274　0.050 -0.650
#10　　90　275　0.050 -0.550
#11　　90　276　0.050 -0.450
#12　　90　278　0.050 -0.350
#13　　90　281　0.050 -0.250
#14　　90　288　0.050 -0.150
#15　　90　315　0.050 -0.050　0度付近で急激な変化にはなるが、
#16　　90　 45　0.050　0.050　平均処理はうまくいっている
#17　　90　 72　0.050　0.150
#18　　90　 79　0.050　0.250
#19　　90　 82　0.050　0.350
#20　　90　 84　0.050　0.450
#21　　90　 85　0.050　0.550
#22　　90　 86　0.050　0.650
#23　　90　 86　0.050　0.750
#24　　90　 87　0.050　0.850
#25　　90　 87　0.050　0.950
#26　　90　 90　0.000　1.000
#27　　90　 90　0.000　1.000

以上、「グルグル回る軸の角度変動の平均値を出す方法」の補足でした。