C#实现一阶卡尔曼滤波算法的示例代码

//FilterKalman.cs

namespace FusionFiltering
{

public class FilterKalman
   {
       private double A = 1;
       private double B = 0;
       private double H = 1;

private double R;
       private double Q;

private double cov = double.NaN;
       private double x = double.NaN;

public FilterKalman(double R, double Q, double A, double B, double H)
       {
           this.R = R;  //过程噪声
           this.Q = Q;  //测量噪声

this.A = A;  //状态转移矩阵
           this.B = B;  //控制矩阵  u为控制向量
           this.H = H;  //将估计范围与单位转化为与系统变量(或者说测量值)一致的范围与单位

this.cov = double.NaN;
           this.x = double.NaN; // estimated signal without noise
       }

public FilterKalman(double R, double Q)
       {
           this.R = R;
           this.Q = Q;
       }

public double filter(double measurement, double u)
       {
           if (double.IsNaN(this.x)) {
               this.x = (1 / this.H) * measurement;
               this.cov = (1 / this.H) * this.Q * (1 / this.H);
           } else {
               double predX = (this.A * this.x) + (this.B * u);
               double predCov = ((this.A * this.cov) * this.A) + this.Q;

// Kalman gain
               double K = predCov * this.H * (1 / ((this.H * predCov * this.H) + this.Q));

// Correction
               this.x = predX + K * (measurement - (this.H * predX));
               this.cov = predCov - (K * this.H * predCov);
           }
           return this.x;
       }

public double filter(double measurement)
       {
           double u = 0;
           if (double.IsNaN(this.x)) {
               this.x = (1 / this.H) * measurement;
               this.cov = (1 / this.H) * this.Q * (1 / this.H);
           } else {
               double predX = (this.A * this.x) + (this.B * u);
               double predCov = ((this.A * this.cov) * this.A) + this.R;

// Kalman gain
               double K = predCov * this.H * (1 / ((this.H * predCov * this.H) + this.Q));

// Correction
               this.x = predX + K * (measurement - (this.H * predX));
               this.cov = predCov - (K * this.H * predCov);
           }
           return this.x;
       }

public double lastMeasurement()
       {
           return this.x;
       }

public void setMeasurementNoise(double noise)
       {
           this.Q = noise;
       }

public void setProcessNoise(double noise)
       {
           this.R = noise;
       }
   }
}

//ProgramTestData.cs

using System;
using System.Linq;

namespace FusionFiltering
{
   public class ProgramTest
   {
       /// <summary>
       /// kalman滤波测试1
       /// </summary>
       [System.Diagnostics.Conditional("DEBUG")]
       public static void TestKalmanFilter1()
       {
           Console.WriteLine("FilterKalman Usage");

FilterKalman test = new FilterKalman(0.008, 0.1);
           double[] testData = { 66, 64, 63, 63, 63, 66, 65, 67, 58 };
           foreach (var x in testData) {
               Console.WriteLine("Input data: {0:#,##0.00}, Filtered data:{1:#,##0.000}", x, test.filter(x));
           }
       }

/// <summary>
       /// Example Usage with controlled input
       /// </summary>
       [System.Diagnostics.Conditional("DEBUG")]
       public static void TestKalmanFilterWithControlled()
       {
           Console.WriteLine("FilterKalman Usage with controlled input");

FilterKalman test = new FilterKalman(0.008, 0.1, 1, 1, 1);
           double[] testData = { 66, 64, 63, 63, 63, 66, 65, 67, 58 };
           double u = 0.2;
           foreach (var x in testData) {
               Console.WriteLine("Input data: {0:#,##0.00}, Filtered data:{1:#,##0.000}", x, test.filter(x, u));
           }

}
   }
}

//Program.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using FusionFiltering;
namespace ConsoleApplication1
{
   class Program
   {
       static void Main(string[] args)
       {

ProgramTest.TestKalmanFilter1();
           Console.ReadKey();
           Console.WriteLine();
           ProgramTest.TestKalmanFilterWithControlled();
           Console.ReadKey();

}
}
}

效果：

来源：https://www.cnblogs.com/Lxk0825/p/13899521.html