【Unscented Kalman Filter】C#无迹卡尔曼滤波Demo--两个示例附代码
示例1用到了ZedGraph进行绘图并保存结果图片。 示例2在WPF下使用Gu.Wpf.DataGrid2D控件显示数据和oxy:Plot控件绘图。
示例1-Console
Result
代码：
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Drawing.Imaging;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using UnscentedKalmanFilter;
using ZedGraph;
namespace Example
{
class Program
{
static void Main(string[] args)
{
var filter = new UKF();//无迹卡尔曼滤波
List measurements = new List();//测量值列表
List states = new List();//预测状态
Random rnd = new Random();
for (int k = 0; k < 100; k++)
{
var measurement = Math.Sin(k * 3.14 * 5 / 180) + (double)rnd.Next(50) / 100;//生成随机测量值
measurements.Add(measurement);
filter.Update(new[] { measurement });//更新状态 更新协方差
states.Add(filter.getState()[0]);//预测状态值添加到列表
}
//ZedGraph.GraphPane https://blog.csdn.net/BADAO_LIUMANG_QIZHI/article/details/99716066
//https://blog.csdn.net/BADAO_LIUMANG_QIZHI/article/details/100112573
//https://blog.csdn.net/qq_31324491/article/details/106634800
GraphPane myPane = new GraphPane(new RectangleF(0, 0, 3200, 2400), "Unscented Kalman Filter", "number", "measurement");//尺寸、标题、x标题、y标题
PointPairList measurementsPairs = new PointPairList();//测量点对列表
PointPairList statesPairs = new PointPairList();//预测点对列表
for (int i = 0; i < measurements.Count; i++)
{
measurementsPairs.Add(i, measurements[i]);
statesPairs.Add(i, states[i]);
}
//绘制曲线
myPane.AddCurve("measurement", measurementsPairs, Color.Red, SymbolType.Circle);//曲线标签，点对数据，颜色，符号类型
myPane.AddCurve("estimate", statesPairs, Color.Green, SymbolType.XCross);
//保存图片
Bitmap bm = new Bitmap(200, 200);
Graphics g = Graphics.FromImage(bm);
myPane.AxisChange(g);
Image im = myPane.Image;
im.Save("result.png", ImageFormat.Png);
}
}
} 示例2-WPF
Result
主窗口界面
主窗口的交互逻辑：
///
/// 主窗口.xaml的交互逻辑
///
public partial class MainWindow : Window
{
public MainWindow()
{
InitializeComponent();
var x = new MainWindowViewModel();//获取视图模型对象
this.DataContext = x;//数据绑定
}
}主窗口的视图模型：
using System;
using System.Collections.ObjectModel;
using UnscentedKalmanFilter;
namespace DemoApp
{
class MainWindowViewModel : INPCBase//视图模型
{
//ObservableCollection 表示一个动态数据集合，它可在添加、删除项目或刷新整个列表时提供通知。
public ObservableCollection Measurements { get; set; }
public ObservableCollection Estimates { get; set; }
public MainWindowViewModel()
{
Measurements = new ObservableCollection();
Estimates = new ObservableCollection();
var filter = new UKF();//无迹卡尔曼滤波
var N = 100;
for (int k = 1; k < N; k++)
{
double[] z = ProcessBuilder.SineWave(k);//计算正弦值 带噪声
filter.Update(z);//滤波
var state = filter.getState();//获取预测装填
var covariance = filter.getCovariance();//获取协方差
Measurements.Add(new Measurement() { Value = z[0], Time = TimeSpan.FromSeconds(k) });//构造测量值对象，添加到测量值集合
Estimates.Add(new Measurement() { Value = state[0], Time = TimeSpan.FromSeconds(k), Variance = covariance[0, 0] });//构造预测值对象，添加到预测值集合
}
}
}
//随机值处理器
public static class ProcessBuilder
{
private static Random rnd = new Random();
public static double[] SineWave(int iteration)//计算正弦值
{
return new[] { Math.Sin(iteration * 3.14 * 5 / 180) + (double)rnd.Next(50) / 100 };
}
}
public struct Measurement//测量
{
private double variance;//协方差
public double Value { get; set; }//值
public TimeSpan Time { get; set; }//时间
public double Variance {
get
{
return variance;
}
set
{
variance = value;//设置协方差的值
UpperDeviation = Value + Math.Sqrt(variance);//上限偏差
LowerDeviation = Value - Math.Sqrt(variance);//下限偏差
}
}
public double UpperDeviation { get;private set; }//获取上偏差
public double LowerDeviation{ get; private set; }//获取下偏差
}
} 属性改变接口：
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace DemoApp
{
public abstract class INPCBase : INotifyPropertyChanged
{
#region INotifyPropertyChanged Implementation
///
/// 在更改此对象上的任何属性时发生。
///
public event PropertyChangedEventHandler PropertyChanged;
///
/// 引发属性的PropertyChanged事件的辅助方法。
///
/// The names of the properties that changed.
public virtual void NotifyChanged(params string[] propertyNames)
{
foreach (string name in propertyNames)
{
OnPropertyChanged(new PropertyChangedEventArgs(name));
}
}
///
/// 引发PropertyChanged事件。
///
/// Event arguments.
protected virtual void OnPropertyChanged(PropertyChangedEventArgs e)
{
if (this.PropertyChanged != null)
{
this.PropertyChanged(this, e);
}
}
#endregion
}
}无迹卡尔曼滤波算法代码：
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using MathNet.Numerics.LinearAlgebra.Factorization;
using System;
namespace UnscentedKalmanFilter
{
public class UKF
{
///
/// 状态值States number
///
private int L;
///
/// 测量值Measurements number
///
private int m;
///
/// 系数，表示均值附近的sigma-point离散
/// The alpha coefficient, characterize sigma-points dispersion around mean
///
private double alpha;
///
/// The ki.
///
private double ki;
///
/// beta系数，表征型分布(2为正态分布)
/// The beta coefficient, characterize type of distribution (2 for normal one)
///
private double beta;
///
/// 比例因子Scale factor
///
private double lambda;
///
/// Scale factor
///
private double c;
///
/// 平均权值Means weights
///
private Matrix Wm;
///
///协方差的权值 Covariance weights
///
private Matrix Wc;
///
/// 状态State
///
private Matrix x;
///
/// 协方差 Covariance
///
private Matrix P;
///
/// Std of process
///
private double q;
///
/// Std of measurement
///
private double r;
///
/// Covariance of process
///
private Matrix Q;
///
/// Covariance of measurement
///
private Matrix R;
///
/// Constructor of Unscented Kalman Filter
/// 无迹卡尔曼滤波的构造函数
///
/// States number
/// Measurements number
public UKF(int L = 0)
{
this.L = L;
}
private void init()
{
q = 0.05;
r = 0.3;
x = q * Matrix.Build.Random(L, 1); //带噪声的初始状态initial state with noise
P = Matrix.Build.Diagonal(L, L, 1); //初始状态协方差 initial state Covariance
Q = Matrix.Build.Diagonal(L, L, q * q); //Covariance of process
R = Matrix.Build.Dense(m, m, r * r); //Covariance of measurement
alpha = 1e-3f;
ki = 0;
beta = 2f;
lambda = alpha * alpha * (L + ki) - L;
c = L + lambda;
//weights for means
Wm = Matrix.Build.Dense(1, (2 * L + 1), 0.5 / c);
Wm[0, 0] = lambda / c;
//weights for covariance
Wc = Matrix.Build.Dense(1, (2 * L + 1));
Wm.CopyTo(Wc);
Wc[0, 0] = Wm[0, 0] + 1 - alpha * alpha + beta;
c = Math.Sqrt(c);
}
public void Update(double[] measurements)
{
if (m == 0)
{
var mNum = measurements.Length;
if (mNum > 0)
{
m = mNum;
if (L == 0) L = mNum;
init();
}
}
var z = Matrix.Build.Dense(m, 1, 0);
z.SetColumn(0, measurements);
//sigma points around x
Matrix X = GetSigmaPoints(x, P, c);
//无迹变换 unscented transformation of process
// X1=sigmas(x1,P1,c) - sigma points around x1
//X2=X1-x1(:,ones(1,size(X1,2))) - deviation of X1
Matrix[] ut_f_matrices = UnscentedTransform(X, Wm, Wc, L, Q);
Matrix x1 = ut_f_matrices[0];
Matrix X1 = ut_f_matrices[1];
Matrix P1 = ut_f_matrices[2];
Matrix X2 = ut_f_matrices[3];
//unscented transformation of measurments
Matrix[] ut_h_matrices = UnscentedTransform(X1, Wm, Wc, m, R);
Matrix z1 = ut_h_matrices[0];
Matrix Z1 = ut_h_matrices[1];
Matrix P2 = ut_h_matrices[2];
Matrix Z2 = ut_h_matrices[3];
//transformed cross-covariance
Matrix P12 = (X2.Multiply(Matrix.Build.Diagonal(Wc.Row(0).ToArray()))).Multiply(Z2.Transpose());
Matrix K = P12.Multiply(P2.Inverse());
//更新状态state update
x = x1.Add(K.Multiply(z.Subtract(z1)));
//更新协方差covariance update
P = P1.Subtract(K.Multiply(P12.Transpose()));
}
public double[] getState()
{
return x.ToColumnArrays()[0];
}
public double[,] getCovariance()
{
return P.ToArray();
}
///
/// Unscented Transformation
///
/// nonlinear map
/// sigma points
/// Weights for means
/// Weights for covariance
/// numer of outputs of f
/// additive covariance
/// [transformed mean, transformed smapling points, transformed covariance, transformed deviations
private Matrix[] UnscentedTransform(Matrix X, Matrix Wm, Matrix Wc, int n, Matrix R)
{
int L = X.ColumnCount;
Matrix y = Matrix.Build.Dense(n, 1, 0);
Matrix Y = Matrix.Build.Dense(n, L, 0);
Matrix row_in_X;
for (int k = 0; k < L; k++)
{
row_in_X = X.SubMatrix(0, X.RowCount, k, 1);
Y.SetSubMatrix(0, Y.RowCount, k, 1, row_in_X);
y = y.Add(Y.SubMatrix(0, Y.RowCount, k, 1).Multiply(Wm[0,k]));
}
Matrix Y1 = Y.Subtract(y.Multiply(Matrix.Build.Dense(1,L,1)));
Matrix P = Y1.Multiply(Matrix.Build.Diagonal(Wc.Row(0).ToArray()));
P = P.Multiply(Y1.Transpose());
P = P.Add(R);
Matrix[] output = { y, Y, P, Y1 };
return output;
}
///
/// Sigma points around reference point
///
/// reference point
/// covariance
/// coefficient
/// Sigma points
private Matrix GetSigmaPoints(Matrix x, Matrix P, double c)
{
Matrix A = P.Cholesky().Factor;
A = A.Multiply(c);
A = A.Transpose();
int n = x.RowCount;
Matrix Y = Matrix.Build.Dense(n, n, 1);
for (int j=0; j X = Matrix.Build.Dense(n,(2*n+1));
X.SetSubMatrix(0, n, 0, 1, x);
Matrix Y_plus_A = Y.Add(A);
X.SetSubMatrix(0, n, 1, n, Y_plus_A);
Matrix Y_minus_A = Y.Subtract(A);
X.SetSubMatrix(0, n, n+1, n, Y_minus_A);
return X;
}
}
} 参考：
https://blog.csdn.net/muxi_huang/article/details/105070498 C#控件ZedGraph使用小
https://blog.csdn.net/BADAO_LIUMANG_QIZHI/article/details/101370197
https://blog.csdn.net/BADAO_LIUMANG_QIZHI/article/details/99716066 https://blog.csdn.net/BADAO_LIUMANG_QIZHI/article/details/100112573
https://blog.csdn.net/qq_31324491/article/details/106634800
https://blog.csdn.net/Frankgoogle/article/details/105525098
https://blog.csdn.net/ouyangliping/article/details/5152476
https://zhuanlan.zhihu.com/p/399370551
https://www.cnblogs.com/long5683/p/14091520.html
https://www.cnblogs.com/yrm1160029237/p/10161663.html
https://cloud.tencent.com/developer/article/2040902
The End