随着传感技术、机器人、自动驾驶以及航空航天等技术的不断发展，对控制系统的精度及稳定性的要求也越来越高。卡尔曼滤波作为一种状态最优估计的方法，其应用也越来越普遍，卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用，下面带来的是它的C#版本。
/// <summary>
/// Simple implementation of the Kalman Filter for 1D data.
/// Originally written in JavaScript by Wouter Bulten
///
/// https://github.com/wouterbulten/kalmanjs/blob/master/contrib/java/KalmanFilter.java
/// </summary>
public class FilterKalman
{
private double A = 1;
private double B = 0;
private double C = 1;
private double R;
private double Q;
private double cov = double.NaN;
private double x = double.NaN;
/// <summary>
/// Constructor
/// </summary>
/// <param name="R">R Process noise</param>
/// <param name="Q">Q Measurement noise</param>
/// <param name="A">A State vector</param>
/// <param name="B">B Control vector</param>
/// <param name="C">C Measurement vector</param>
public FilterKalman(double R, double Q, double A, double B, double C)
{
this.R = R;
this.Q = Q;
this.A = A;
this.B = B;
this.C = C;
this.cov = double.NaN;
this.x = double.NaN; // estimated signal without noise
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="R">R Process noise</param>
/// <param name="Q">Q Measurement noise</param>
public FilterKalman(double R, double Q)
{
this.R = R;
this.Q = Q;
}
/// <summary>
/// Filters a measurement
/// </summary>
/// <param name="measurement">The measurement value to be filtered</param>
/// <param name="u">The controlled input value</param>
/// <returns>The filtered value</returns>
public double filter(double measurement, double u)
{
if (double.IsNaN(this.x))
{
this.x = (1 / this.C) * measurement;
this.cov = (1 / this.C) * this.Q * (1 / this.C);
}
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.C * (1 / ((this.C * predCov * this.C) + this.Q));
// Correction
this.x = predX + K * (measurement - (this.C * predX));
this.cov = predCov - (K * this.C * predCov);
}
return this.x;
}
/// <summary>
/// Filters a measurement
/// </summary>
/// <param name="measurement">The measurement value to be filtered</param>
/// <returns>The filtered value</returns>
public double filter(double measurement)
{
double u = 0;
if (double.IsNaN(this.x))
{
this.x = (1 / this.C) * measurement;
this.cov = (1 / this.C) * this.Q * (1 / this.C);
}
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.C * (1 / ((this.C * predCov * this.C) + this.Q));
// Correction
this.x = predX + K * (measurement - (this.C * predX));
this.cov = predCov - (K * this.C * predCov);
}
return this.x;
}
/// <summary>
/// Set the last measurement.
/// </summary>
/// <returns>return The last measurement fed into the filter</returns>
public double lastMeasurement()
{
return this.x;
}
/// <summary>
/// Sets measurement noise
/// </summary>
/// <param name="noise">The new measurement noise</param>
public void setMeasurementNoise(double noise)
{
this.Q = noise;
}
/// <summary>
/// Sets process noise
/// </summary>
/// <param name="noise">The new process noise</param>
public void setProcessNoise(double noise)
{
this.R = noise;
}
}
应用示例如下：
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));
}
Console.WriteLine("FilterKalman Usage with controlled input");
FilterKalman test2 = new FilterKalman(0.008, 0.1, 1, 1, 1);
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));
}
可以下载我写的源码：
链接：https://pan.baidu.com/s/1GqfJJF4I1PLJkx7uCIwOfQ
💰 此内容为付费阅读 请先登录
