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

//FilterKalman.cs namespace FusionFiltering { public class FilterKalman { private double A = 1; private double B = 0;

```//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)
{
http://www.cppcns.com 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) * thisdDauMP.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));
http://www.cppcns.com        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 FusionFiltering;
namespace ConsoleApplication1
{
class Program
{
static void Main(string[] args)
{

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