过滤流数据以减少噪声，卡尔曼滤波c#

我正在将数据从惯性传感器传输到C#应用程序中。数据有点嘈杂，所以我需要添加一个过滤器来平滑它。我有一个kalman滤波器实现，当给定一个数组时效果很好，但我无法理解如何在恒定的数据流中使用它

我有：

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]);
                }
            }

publicstaticdouble[]noisesine=newdouble[20]{40,41,38,40,45,42,43,44,40,38,44,45,40,39,37,41,42,70,44,42}；
公共静态双精度[]干净=新双精度[20]；
公共静态真空Kalman滤波器（双[]噪声）
{                  
double A=double.Parse（“1”）；//实值到上一个实值的因子
//双B=0；//实际值对实际控制信号的系数
双H=double.Parse（“1”）；
双P=double.Parse（“0.1”）；
double Q=double.Parse（“0.125”）；//进程噪声。
double R=double.Parse（“1”）；//假定环境噪声。
双K；
双z；
双x；
//分配给第一个测量值
x=噪音[0]；
for（int i=0；i

如何将一个double（而不是数组）流式输入并返回一个（过滤的）double

谢谢。

试试下面的代码

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));
            }
        }
    }
}

使用系统；
使用System.Collections.Generic；
使用System.Linq；
使用系统文本；
使用System.IO；
命名空间控制台应用程序1
{
班级计划
{
静态void Main（字符串[]参数）
{
双[]输入={1.1,2.2,3.3,4.4}；
byte[]bArray=input.Select（x=>BitConverter.GetBytes（x））.SelectMany（y=>y）.ToArray（）；
MemoryStream inStream=新的MemoryStream（bArray）；
长长度=河道内长度；
字节[]输出数组=新字节[长度]；
流内读取（输出数组，0，（int）长度）；
列表输出=新列表（）；
对于（int i=0；i

创建此类：

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]);

KalmanFilter filter=新的KalmanFilter（1,1,0.125,1,0.1，noisesine[0]）；
对于（inti=0；i

这就是如何修改代码以流式传输双精度输入，并返回过滤后的双精度输入

  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;
        }

publicstaticvoidkalmantest（）
{
double[]noisesine=新的double[20]{40,41,38,40,45,42,43,44,40,38,44,45,40,39,37,41,42,70,44,42}；
对于（int i=0；i

注意：有一个bug。当该代码迭代时，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;
        }

//指定默认值
public static double clean=double.Parse（“0”）；//任何价值
公共静态双Kalman滤波器（双噪声）
{                  
double K=double.Parse（“0.125”）；//噪声0

double是八个字节。要流式传输数据，您需要一个字节数组。所以使用Bit.Converter类。嗨，谢谢你的回复。我不明白你的意思。我有一个变量（双）不断更新。我需要将其发送到一个过滤器函数中，该函数当前使用双[]。@anti您解决过这个问题吗？此实现中存在一个错误：当此代码迭代时，P很快变成接近R/100000的值，并且与噪声无关（在计算中没有对噪声或稳定读数的引用），谢谢，但我不认为这是我需要的。这如何帮助我将数据流发送到上面的过滤器函数中？如果我遗漏了什么，请道歉！我更新了代码以显示所有需要的代码。我使用了一个内存流。这不是一个bug，P（估计不确定性）为0表示错误