示例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<double> measurements = new List<double>();//测量值列表List<double> states = new List<double>();//预测状态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/106634800GraphPane 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

主窗口界面

<Window x:Class="DemoApp.MainWindow"xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"xmlns:d="http://schemas.microsoft.com/expression/blend/2008"xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"xmlns:local="clr-namespace:DemoApp"xmlns:oxy="http://oxyplot.org/wpf"xmlns:dg2d="http://gu.se/DataGrid2D"mc:Ignorable="d"Title="MainWindow" Height="350" Width="525"><DockPanel><Grid DockPanel.Dock="Bottom" ><Grid HorizontalAlignment="Stretch" VerticalAlignment="Stretch" Grid.Column="1"><TabControl><TabItem Header="Graph"><oxy:Plot  x:Name="Plot1" Grid.Column="0"  ><oxy:Plot.Series><oxy:AreaSeries ItemsSource="{Binding Estimates}"  Title="Estimate Deviation"    DataFieldX="Time" DataFieldY="LowerDeviation" DataFieldX2="Time" DataFieldY2="UpperDeviation" /><oxy:LineSeries ItemsSource="{Binding Measurements}"   Title="Measurement"   DataFieldX="Time" DataFieldY="Value"  MarkerType="Cross"/><oxy:LineSeries ItemsSource="{Binding Estimates}"  Title="Estimate"    DataFieldX="Time" DataFieldY="Value"  /></oxy:Plot.Series><oxy:Plot.Axes><oxy:LinearAxis Position="Bottom"/><oxy:LinearAxis Position="Left"/></oxy:Plot.Axes></oxy:Plot></TabItem><TabItem Header="DataTable"><DataGrid  Grid.Column="1" x:Name="DataGrid1" HorizontalAlignment="Stretch" VerticalAlignment="Stretch"ItemsSource="{Binding Measurements}"/></TabItem></TabControl>      </Grid></Grid></DockPanel>
</Window>

主窗口的交互逻辑：

/// <summary>/// 主窗口.xaml的交互逻辑 /// </summary>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<Measurement> Measurements { get; set; }public ObservableCollection<Measurement> Estimates { get; set; }public MainWindowViewModel(){Measurements = new ObservableCollection<Measurement>();Estimates = new ObservableCollection<Measurement>();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/// <summary>/// 在更改此对象上的任何属性时发生。/// </summary>public event PropertyChangedEventHandler PropertyChanged;/// <summary>/// 引发属性的PropertyChanged事件的辅助方法。/// </summary>/// <param name="propertyNames">The names of the properties that changed.</param>public virtual void NotifyChanged(params string[] propertyNames){foreach (string name in propertyNames){OnPropertyChanged(new PropertyChangedEventArgs(name));}}/// <summary>/// 引发PropertyChanged事件。/// </summary>/// <param name="e">Event arguments.</param>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{/// <summary>/// 状态值States number/// </summary>private int L;  /// <summary>/// 测量值Measurements number/// </summary>private int m;/// <summary>/// 系数，表示均值附近的sigma-point离散/// The alpha coefficient, characterize sigma-points dispersion around mean/// </summary>private double alpha;  /// <summary>/// The ki./// </summary>private double ki;/// <summary>/// beta系数，表征型分布(2为正态分布)/// The beta coefficient, characterize type of distribution (2 for normal one) /// </summary>private double beta;/// <summary>/// 比例因子Scale factor/// </summary>private double lambda;/// <summary>/// Scale factor/// </summary>private double c; /// <summary>/// 平均权值Means weights/// </summary>private Matrix<double> Wm;/// <summary>///协方差的权值 Covariance weights/// </summary>private Matrix<double> Wc;/// <summary>/// 状态State/// </summary>private Matrix<double> x;/// <summary>/// 协方差 Covariance/// </summary>private Matrix<double> P;/// <summary>/// Std of process /// </summary>private double q;/// <summary>/// Std of measurement /// </summary>private double r;/// <summary>/// Covariance of process/// </summary>private Matrix<double> Q;/// <summary>/// Covariance of measurement /// </summary>private Matrix<double> R;/// <summary>/// Constructor of Unscented Kalman Filter/// 无迹卡尔曼滤波的构造函数/// </summary>/// <param name="L">States number</param>/// <param name="m">Measurements number</param>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 noiseP = Matrix.Build.Diagonal(L, L, 1); //初始状态协方差 initial state CovarianceQ = Matrix.Build.Diagonal(L, L, q * q); //Covariance of processR = 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 meansWm = Matrix.Build.Dense(1, (2 * L + 1), 0.5 / c);Wm[0, 0] = lambda / c;//weights for covarianceWc = 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 xMatrix<double> 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 X1Matrix<double>[] ut_f_matrices = UnscentedTransform(X, Wm, Wc, L, Q);Matrix<double> x1 = ut_f_matrices[0];Matrix<double> X1 = ut_f_matrices[1];Matrix<double> P1 = ut_f_matrices[2];Matrix<double> X2 = ut_f_matrices[3];//unscented transformation of measurmentsMatrix<double>[] ut_h_matrices = UnscentedTransform(X1, Wm, Wc, m, R);Matrix<double> z1 = ut_h_matrices[0];Matrix<double> Z1 = ut_h_matrices[1];Matrix<double> P2 = ut_h_matrices[2];Matrix<double> Z2 = ut_h_matrices[3];//transformed cross-covarianceMatrix<double> P12 = (X2.Multiply(Matrix.Build.Diagonal(Wc.Row(0).ToArray()))).Multiply(Z2.Transpose());Matrix<double> K = P12.Multiply(P2.Inverse());//更新状态state updatex = 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();}/// <summary>/// Unscented Transformation/// </summary>/// <param name="f">nonlinear map</param>/// <param name="X">sigma points</param>/// <param name="Wm">Weights for means</param>/// <param name="Wc">Weights for covariance</param>/// <param name="n">numer of outputs of f</param>/// <param name="R">additive covariance</param>/// <returns>[transformed mean, transformed smapling points, transformed covariance, transformed deviations</returns>private Matrix<double>[] UnscentedTransform(Matrix<double> X, Matrix<double> Wm, Matrix<double> Wc, int n, Matrix<double> R){int L = X.ColumnCount;Matrix<double> y = Matrix.Build.Dense(n, 1, 0);Matrix<double> Y = Matrix.Build.Dense(n, L, 0);Matrix<double> 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<double> Y1 = Y.Subtract(y.Multiply(Matrix.Build.Dense(1,L,1)));Matrix<double> P = Y1.Multiply(Matrix.Build.Diagonal(Wc.Row(0).ToArray()));P = P.Multiply(Y1.Transpose());P = P.Add(R);Matrix<double>[] output = { y, Y, P, Y1 };return output;}/// <summary>/// Sigma points around reference point/// </summary>/// <param name="x">reference point</param>/// <param name="P">covariance</param>/// <param name="c">coefficient</param>/// <returns>Sigma points</returns>private Matrix<double> GetSigmaPoints(Matrix<double> x, Matrix<double> P, double c) {Matrix<double> A = P.Cholesky().Factor;A = A.Multiply(c);A = A.Transpose();int n = x.RowCount;Matrix<double> Y = Matrix.Build.Dense(n, n, 1);for (int j=0; j<n; j++)  {Y.SetSubMatrix(0, n, j, 1, x);}Matrix<double> X = Matrix.Build.Dense(n,(2*n+1));X.SetSubMatrix(0, n, 0, 1, x);Matrix<double> Y_plus_A = Y.Add(A);  X.SetSubMatrix(0, n, 1, n, Y_plus_A);Matrix<double> 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