[JAVA] How to find the average angle

Premise

The target is the angle in the two-dimensional coordinate system I am writing a program with an angle of -180 ° to 180 °, but I think that it can be 0 to 360 ° with a little rewriting.

Background (You can skip it)

In my master's research, I am making an indoor positioning application using Android (Java), and I am using a particle filter in it. Among them, it was necessary to take the operation of averaging the state vectors of each particle. There are three state vectors (x, y, θ), of which (x, y) can be simply divided by the number, but the angle does not work due to the problem of periodicity (described later). So I made it myself

Do not simply find the average angle

It is explained in detail with figures on the following site http://d.hatena.ne.jp/ootanAW/20111030/1319981509 Simply put, the average of 350 ° and 10 ° (at the angle defined by [0 ° ~ 360 °]) is 180 °, right? But considering the periodicity, I want the average to be 0 °. This time I am thinking about [-180 ° ~ 180 °], but the same problem occurs.

Then what should we do

According to the site of ↓, there seems to be a method of once converting the angle to a vector on the unit circle, synthesizing it, averaging the vectors, and then performing the inverse conversion. https://staff.aist.go.jp/toru-nakata/averageangle.html

The procedure is as follows

Generate a vector ** Vn ** of (cos θn, sin θn) from each angle θn
Find the average vector ** V ** of ** Vn **
Obtain the average angle θ from ** V **

Code created (Java)

That's why I wrote it It is also uploaded to gist, so feel free to use it (I would appreciate it if you could point out any deficiencies).

`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("Average angle");

        //Variable declaration / initialization
        double ang_avg;

        // -----Enter the angle you want to average here-----
        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]);
        }

        //Find the average
        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);

        //Correction of calculation error
        if(Math.abs(ang_avg) < Math.pow(10, -8)){
            ang_avg = 0.0;
        }

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