角度の平均を求める方法

前提

対象は二次元座標系での角度です
角度は-180° ~ 180°でプログラムを書いていますが、少し書き換えれば0 ~ 360°でもいけると思います

背景 (読み飛ばしても大丈夫です)

修士研究でAndroidを使って屋内測位アプリを作っていて(Java)、その中でパーティクルフィルタというのを使っています
その中で、各パーティクルの状態ベクトルの平均をとる操作が必要でした。状態ベクトルには(x,y,θ)の3つあって、そのうち(x,y)は単純に総和を個数で割ればいいのですが、角度は周期性の問題（後述）で上手くいきません
なので、ちょっと自作してみました

角度の平均は単純に求めてはいけない

以下のサイトで図付きで詳しく説明されています
http://d.hatena.ne.jp/ootanAW/20111030/1319981509
簡単に言うと、（[0° ~ 360°]で定義される角で）350°と10°の平均って180°になっちゃいますよね？でも、周期性を考慮すると平均は0°であってほしいです
今回は[-180° ~ 180°]で考えていますが、同様の問題が発生します

ではどうするか

↓のサイトによると、一旦角度を単位円上のベクトルに変換して合成し、ベクトルの平均をとってから逆変換するという方法があるようです
https://staff.aist.go.jp/toru-nakata/averageangle.html

手順としては以下

1. 各角度θnから(cosθn, sinθn)のベクトルVnを生成
2. Vnの平均ベクトルVを求める
3. Vから平均角度θを求める

作成したコード(Java)

というわけで書いてみました
gistにもアップしているので自由に使ってください(不備などありましたらご指摘いただけると幸いです)

Angle_average.java

import java.lang.Math;
public class Angle_average{
    public static void main(String[] args){

        final class vector{
            double x;
            double y;
        }

        System.out.println("角度の平均");

        // 変数宣言・初期化
        double ang_avg;

        // -----ここに平均を求めたい角度を入力-----
        double[] angles = {10.0, 30.0, -10.0, -30.0};

        int len = angles.length;

        double[] angles_rad = new double[len];

        vector[] vectors = new vector[len];

        vector vector_avg = new vector();

        for (int i = 0; i < vectors.length; i++) {
            vectors[i] = new vector();
        }

        for (int i = 0; i < angles.length; i++) {
            angles_rad[i] = angles[i] * (Math.PI / 180.0);

            vectors[i].x = Math.cos(angles_rad[i]);
            vectors[i].y = Math.sin(angles_rad[i]);
        }

        // 平均を求める
        vector_avg.x = 0.0;
        vector_avg.y = 0.0;

        for (int i = 0; i < vectors.length; i++) {
            vector_avg.x += vectors[i].x;
            vector_avg.y += vectors[i].y;
        }

        vector_avg.x = vector_avg.x / (double)vectors.length;
        vector_avg.y = vector_avg.y / (double)vectors.length;
        
        ang_avg = Math.atan2(vector_avg.y, vector_avg.x);

        // 計算誤差の補正
        if(Math.abs(ang_avg) < Math.pow(10, -8)){
            ang_avg = 0.0;
        }

        System.out.println(Double.toString(Math.toDegrees(ang_avg)) + "\n");
    }
}