ノイズを減らすためのストリーミングデータのフィルタリング、カルマンフィルタC＃

Question

私は、慣性センサからC＃アプリケーションにデータをストリーミングしています。データは少しノイズが多いので、フィルタを追加してそれを滑らかにする必要があります。私は、配列を与えられたときにうまくいくカルマンフィルタの実装を持っていますが、定数のデータストリームでどのように使用するかについて頭を悩ますことはできません。

私が持っている：

double sensorData; //the noisy value, constantly updating from another class. 

フィルタ：

public static double[] noisySine = new double[20] { 40, 41, 38, 40, 45, 42, 43, 44, 40, 38, 44, 45, 40, 39, 37, 41, 42, 70, 44, 42 }; 
    public static double[] clean = new double[20]; 

     public static void KalmanFilter(double[] noisy) 
      {     
       double A = double.Parse("1"); //factor of real value to previous real value 
       // double B = 0; //factor of real value to real control signal 
       double H = double.Parse("1"); 
       double P = double.Parse("0.1"); 
       double Q = double.Parse("0.125"); //Process noise. 
       double R = double.Parse("1"); //assumed environment noise. 
       double K; 
       double z; 
       double x; 

       //assign to first measured value 
       x = noisy[0]; 
       for (int i = 0; i < noisy.Length; i++) 
       { 
        //get current measured value 
        z = noisy[i]; 

        //time update - prediction 
        x = A * x; 
        P = A * P * A + Q; 

        //measurement update - correction 
        K = P * H/(H * P * H + R); 
        x = x + K * (z - H * x); 
        P = (1 - K * H) * P; 
        //estimated value 
        clean[i] = x; 
        Console.WriteLine(noisy[i] + " " + clean[i]); 
       } 
      } 

は、どのように私は、ダブル配列の代わりに、中に二重のストリーミング、および（フィルタリング）を返すことができますか？

ありがとうございます。

Answer 1

このクラスを作成し、次のコード

using System; 
using System.Collections.Generic; 
using System.Linq; 
using System.Text; 
using System.IO; 

namespace ConsoleApplication1 
{ 
    class Program 
    { 
     static void Main(string[] args) 
     { 
      double[] input = {1.1,2.2,3.3,4.4}; 
      byte[] bArray = input.Select(x => BitConverter.GetBytes(x)).SelectMany(y => y).ToArray(); 
      MemoryStream inStream = new MemoryStream(bArray); 
      long length = inStream.Length; 
      byte[] outArray = new byte[length]; 
      inStream.Read(outArray, 0, (int)length); 
      List<double> output = new List<double>(); 
      for (int i = 0; i < bArray.Length; i += 8) 
      { 
       output.Add(BitConverter.ToDouble(outArray,i)); 
      } 
     } 
    } 
} 

Answer 2

をお試しください：

public class KalmanFilter 
{ 
    private double A, H, Q, R, P, x; 

    public KalmanFilter(double A, double H, double Q, double R, double initial_P, double initial_x) 
    { 
     this.A = A; 
     this.H = H; 
     this.Q = Q; 
     this.R = R; 
     this.P = initial_P; 
     this.x = initial_x; 
    } 

    public double Output(double input) 
    { 
     // time update - prediction 
     x = A * x; 
     P = A * P * A + Q; 

     // measurement update - correction 
     double K = P * H/(H * P * H + R); 
     x = x + K * (input - H * x); 
     P = (1 - K * H) * P; 

     return x; 
    } 
} 

とクラスを使用します。

KalmanFilter filter = new KalmanFilter(1, 1, 0.125, 1, 0.1, noisySine[0]); 
for (int i = 0; i < noisy.Length; i++) clean[i] = filter.Output(noisySine[i]); 

Answer 3

これはあなたがダブルをストリーミングするためにコードを変更するにはどうすればよいですフィルタリングされたdoubleを返します。

public static void KalmanTest() 
     { 
      double[] noisySine = new double[20] { 40, 41, 38, 40, 45, 42, 43, 44, 40, 38, 44, 45, 40, 39, 37, 41, 42, 70, 44, 42 }; 
      for (int i = 0; i < noisySine.Length; i++) 
      { 
       Console.WriteLine(noisySine[i] + " " + KalmanFilter(noisySine[i])); 
      } 
     } 


    // assign default values 
    // for a new mwasurement, reset this values 
    public static double P = double.Parse("1"); // MUST be greater than 0 
    public static double clean = double.Parse("0"); // any value 

    public static double KalmanFilter(double noisy) 
     {     
      double A = double.Parse("1"); //factor of real value to previous real value 
      // double B = 0; //factor of real value to real control signal 
      double H = double.Parse("1"); 
      double Q = double.Parse("0.125"); //Process noise. 
      double R = double.Parse("1"); //assumed environment noise. 
      double K; 
      double z; 
      double x; 

       //get current measured value 
       z = noisy; 

       //time update - prediction 
       x = A * clean; 
       P = A * P * A + Q; 

       //measurement update - correction 
       K = P * H/(H * P * H + R); 
       x = x + K * (z - H * x); 
       P = (1 - K * H) * P; 
       //estimated value 
       clean = x; 
       return clean; 
     } 

注：バグがあります。このコードが繰り返し実行されると、PはすぐにR/100000に近い値になり、この計算はノイズに依存しません。なぜなら、Pの計算ではノイズの多いまたは安定した測定値への参照がないからです。 クリーンコードはローパスフィルタのように見えます。

// assign default values 
    public static double clean = double.Parse("0"); // any value 

    public static double KalmanFilter(double noisy) 
     {     
      double K = double.Parse("0.125"); // noise 0 < K < 1 
      clean = clean + K * (noisy - clean); 
      return clean; 
     }