C#实现卡尔曼滤波器去噪（简易版）

原理

  网上资料很多，不再赘述，详情了解：
  https://blog.csdn.net/panjinliang066333/article/details/105863642/

代码

注：该方法选用所有数据作为输入计算其后验均值与方差

    public static double[] kalman_Filter(string filePath, double dataNum, double R, double Q)
    {
        System.IO.StreamReader readerFile = new System.IO.StreamReader(filePath);//读取测量数据文件
        string str = readerFile.ReadToEnd();//获取文件中数据
        Array array = str.Split('\n');//数据分割
         //初始化参数
        int N = array.Length - 1;
        double[] y = new double[N];
        double average=0, sum = 0, variance_sum=0, variance=0;

        //文件数据写进数组
        for (int a = 0; a < array.Length - 1; a++)
        {
            y[a] = Convert.ToDouble(array.GetValue(a));
        }

        //定义参数矩阵
        double[] K = new double[N];//卡尔曼增益矩阵
        double[] P = new double[N];//后验误差估计协方差矩阵
        double[] X = new double[N];//结果矩阵

        //有测量原始数据计算后验测量方差
        for (int n = 0; n < dataNum; n++)
        {
            sum += y[n];
        }
        average = sum / dataNum;//求均值
        for (int m = 0; m < dataNum; m++)
        {
            variance_sum += Math.Sqrt(Math.Abs(y[m] - average));
        }
        variance = variance_sum / dataNum;//求方差

        //矩阵初始化赋值
        X[0] = average;
        P[0] = variance;
        
        //滤波过程
        for (int i = 1; i < N; i++)
        {
            K[i] = P[i - 1] / (P[i - 1] + R);
            X[i] = X[i - 1] + K[i] * (y[i] - X[i - 1]);
            P[i] = P[i - 1] - K[i] * P[i - 1] + Q;
        }
        
        readerFile.Close();
        return X;
    }

如果数据读取不想用IO流，这直接把输入参数改为数组，具体代码如下：

    public static double[] kalmanFilter(double[] array, double R, double Q)
    {
        int N = array.Length;
        double average = 0, sum = 0, variance_sum = 0, variance = 0;
        double[] K = new double[N];
        double[] P = new double[N];
        double[] X = new double[N]; 
        for (int n = 0; n < N; n++)
        {
            sum += array[n];
        }
        average = sum / N;
        for (int m = 0; m < N; m++)
        {
            variance_sum += Math.Sqrt(Math.Abs(array[m] - average));
        }
        variance = variance_sum / N;

        //矩阵初始化赋值
        X[0] = average;
        P[0] = variance;

        //滤波过程
        for (int i = 1; i < N; i++)
        {
            K[i] = P[i - 1] / (P[i - 1] + R);
            X[i] = X[i - 1] + K[i] * (array[i] - X[i - 1]);
            P[i] = P[i - 1] - K[i] * P[i - 1] + Q;
        }
        return X;
    }