【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<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/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

主窗口界面

<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 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<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 X1
            Matrix<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 measurments
            Matrix<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-covariance
            Matrix<double> P12 = (X2.Multiply(Matrix.Build.Diagonal(Wc.Row(0).ToArray()))).Multiply(Z2.Transpose());


            Matrix<double> 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();
        }


        /// <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