namespace IBox.WinFrmUI
|
{
|
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()
|
{
|
|
}
|
/// <summary>
|
/// 卡尔曼滤波
|
/// </summary>
|
public FilterKalman(double R, double Q, double A, double B, double H)
|
{
|
this.R = R;
|
this.Q = Q;
|
|
this.A = A;
|
this.B = B;
|
this.H = H;
|
|
this.cov = double.NaN;
|
this.x = double.NaN;
|
}
|
|
/// <summary>
|
///
|
/// </summary>
|
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;
|
|
double K = predCov * this.H * (1 / ((this.H * predCov * this.H) + this.Q));
|
|
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;
|
|
double K = predCov * this.H * (1 / ((this.H * predCov * this.H) + this.Q));
|
|
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;
|
}
|
}
|
}
|
