Javaによるカルマンフィルターの実装方法

なんでもフィルタリングできて手軽に実装できるカルマンフィルター！
Javaでの実装例がなかったのでメモとして書いておきます。

    public static void main(String[] args){
        ArrayList<Double> result = new ArrayList<Double>();
        double now = 0;
        double xPre = 0;
        double pPre = 0;
        double  sigmaW = 0;
        double sigmaV = 0;
        //引数指定　1 sigmaW , 2 sigmaV
        sigmaW = Integer.parseInt("5");
        sigmaV = Integer.parseInt("50");
        Scanner sc  = new Scanner(System.in);
        while(true){

            now = sc.nextDouble();
            if(now == -1){
                break;
            }
            double xForecast = xPre;
            double pForecast = pPre + sigmaW;
            double KGain = pForecast/(pForecast + sigmaV);
            double  xFiltered = (xForecast + KGain*(now - xForecast));
            double pFiletered = (1-KGain) * pForecast;
            result.add(xFiltered);
            System.out.println(xFiltered);
            xPre = xFiltered;
            pPre = pFiletered;
        }
        System.out.println("-------------------------------");
        for(Double out : result){
            System.out.println(out);

        }

    }

sigmaWとsigmaVの組み合わせによってフィルタリングの度合いが変わってきます。

これを利用してAPI化することによって様々な数値をフィルタリングさせることができます。

ここではリストとして出力しているがリアルタイムで値を出力させることもできます。