过滤流数据减少噪声,卡尔曼滤波c#
本文关键字:卡尔曼滤波 噪声 数据 过滤 | 更新日期: 2023-09-27 18:05:29
我从一个惯性传感器流数据到c#应用程序。数据有点嘈杂,所以我需要添加一个过滤器来平滑它。当给定数组时,我有一个卡尔曼滤波器实现,它工作得很好,但我不知道如何在恒定数据流上使用它。
我有:
double sensorData; //the noisy value, constantly updating from another class.
过滤器:
public static double[] noisySine = new double[20] { 40, 41, 38, 40, 45, 42, 43, 44, 40, 38, 44, 45, 40, 39, 37, 41, 42, 70, 44, 42 };
public static double[] clean = new double[20];
public static void KalmanFilter(double[] noisy)
{
double A = double.Parse("1"); //factor of real value to previous real value
// double B = 0; //factor of real value to real control signal
double H = double.Parse("1");
double P = double.Parse("0.1");
double Q = double.Parse("0.125"); //Process noise.
double R = double.Parse("1"); //assumed environment noise.
double K;
double z;
double x;
//assign to first measured value
x = noisy[0];
for (int i = 0; i < noisy.Length; i++)
{
//get current measured value
z = noisy[i];
//time update - prediction
x = A * x;
P = A * P * A + Q;
//measurement update - correction
K = P * H / (H * P * H + R);
x = x + K * (z - H * x);
P = (1 - K * H) * P;
//estimated value
clean[i] = x;
Console.WriteLine(noisy[i] + " " + clean[i]);
}
}
我怎么能流一个双精度,而不是一个数组,并返回一个(过滤)双精度?
谢谢。
创建这个类:
public class KalmanFilter
{
private double A, H, Q, R, P, x;
public KalmanFilter(double A, double H, double Q, double R, double initial_P, double initial_x)
{
this.A = A;
this.H = H;
this.Q = Q;
this.R = R;
this.P = initial_P;
this.x = initial_x;
}
public double Output(double input)
{
// time update - prediction
x = A * x;
P = A * P * A + Q;
// measurement update - correction
double K = P * H / (H * P * H + R);
x = x + K * (input - H * x);
P = (1 - K * H) * P;
return x;
}
}
并使用类:
KalmanFilter filter = new KalmanFilter(1, 1, 0.125, 1, 0.1, noisySine[0]);
for (int i = 0; i < noisy.Length; i++) clean[i] = filter.Output(noisySine[i]);
试试下面的代码
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
namespace ConsoleApplication1
{
class Program
{
static void Main(string[] args)
{
double[] input = {1.1,2.2,3.3,4.4};
byte[] bArray = input.Select(x => BitConverter.GetBytes(x)).SelectMany(y => y).ToArray();
MemoryStream inStream = new MemoryStream(bArray);
long length = inStream.Length;
byte[] outArray = new byte[length];
inStream.Read(outArray, 0, (int)length);
List<double> output = new List<double>();
for (int i = 0; i < bArray.Length; i += 8)
{
output.Add(BitConverter.ToDouble(outArray,i));
}
}
}
}
这是你如何修改你的代码来传入一个双精度类型,并返回一个过滤后的双精度类型。
public static void KalmanTest()
{
double[] noisySine = new double[20] { 40, 41, 38, 40, 45, 42, 43, 44, 40, 38, 44, 45, 40, 39, 37, 41, 42, 70, 44, 42 };
for (int i = 0; i < noisySine.Length; i++)
{
Console.WriteLine(noisySine[i] + " " + KalmanFilter(noisySine[i]));
}
}
// assign default values
// for a new mwasurement, reset this values
public static double P = double.Parse("1"); // MUST be greater than 0
public static double clean = double.Parse("0"); // any value
public static double KalmanFilter(double noisy)
{
double A = double.Parse("1"); //factor of real value to previous real value
// double B = 0; //factor of real value to real control signal
double H = double.Parse("1");
double Q = double.Parse("0.125"); //Process noise.
double R = double.Parse("1"); //assumed environment noise.
double K;
double z;
double x;
//get current measured value
z = noisy;
//time update - prediction
x = A * clean;
P = A * P * A + Q;
//measurement update - correction
K = P * H / (H * P * H + R);
x = x + K * (z - H * x);
P = (1 - K * H) * P;
//estimated value
clean = x;
return clean;
}
注意:有一个bug。当此代码迭代时,P很快变成接近R/100000的值,并且这种行为与噪声无关,因为在P计算中没有引用噪声或稳定读数。干净的代码看起来像一个低通滤波器:
// assign default values
public static double clean = double.Parse("0"); // any value
public static double KalmanFilter(double noisy)
{
double K = double.Parse("0.125"); // noise 0 < K < 1
clean = clean + K * (noisy - clean);
return clean;
}