refactoring

prozoroff · Aug 24, 2017 · bf5ea1b · bf5ea1b
1 parent 73b8110
commit bf5ea1b
Show file tree

Hide file tree

Showing 12 changed files with 141 additions and 190 deletions.
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1,8 @@
+UnscentedKalmanFilter/obj/*
+packages/*
+Example/bin/*
+DemoApp/obj/*
+Example/obj/*
+UnscentedKalmanFilter/bin/*
+DemoApp/bin/*
+.vs/*
diff --git a/DemoApp/DemoApp.csproj b/DemoApp/DemoApp.csproj
@@ -35,17 +35,17 @@
     <WarningLevel>4</WarningLevel>
   </PropertyGroup>
   <ItemGroup>
-    <Reference Include="DataGrid2DLibrary">
-      <HintPath>..\..\..\WPF Framework Extensions\DataGrid2DTest\DataGrid2DLibrary\obj\Debug\DataGrid2DLibrary.dll</HintPath>
+    <Reference Include="Gu.Wpf.DataGrid2D, Version=0.2.1.0, Culture=neutral, processorArchitecture=MSIL">
+      <HintPath>..\packages\Gu.Wpf.DataGrid2D.0.2.1.0\lib\net45\Gu.Wpf.DataGrid2D.dll</HintPath>
     </Reference>
     <Reference Include="MathNet.Numerics">
       <HintPath>..\UnscentedKalmanFilter\bin\Debug\MathNet.Numerics.dll</HintPath>
     </Reference>
-    <Reference Include="OxyPlot">
-      <HintPath>..\..\..\Oxyplot-develop\oxyplot-develop\Source\OxyPlot.Wpf\bin\Debug\OxyPlot.dll</HintPath>
+    <Reference Include="OxyPlot, Version=1.0.0.0, Culture=neutral, PublicKeyToken=638079a8f0bd61e9, processorArchitecture=MSIL">
+      <HintPath>..\packages\OxyPlot.Core.1.0.0\lib\net45\OxyPlot.dll</HintPath>
     </Reference>
-    <Reference Include="OxyPlot.Wpf">
-      <HintPath>..\..\..\Oxyplot-develop\oxyplot-develop\Source\OxyPlot.Wpf\bin\Debug\OxyPlot.Wpf.dll</HintPath>
+    <Reference Include="OxyPlot.Wpf, Version=1.0.0.0, Culture=neutral, PublicKeyToken=75e952ba404cdbb0, processorArchitecture=MSIL">
+      <HintPath>..\packages\OxyPlot.Wpf.1.0.0\lib\net45\OxyPlot.Wpf.dll</HintPath>
     </Reference>
     <Reference Include="System" />
     <Reference Include="System.Data" />
@@ -100,6 +100,7 @@
       <Generator>ResXFileCodeGenerator</Generator>
       <LastGenOutput>Resources.Designer.cs</LastGenOutput>
     </EmbeddedResource>
+    <None Include="packages.config" />
     <None Include="Properties\Settings.settings">
       <Generator>SettingsSingleFileGenerator</Generator>
       <LastGenOutput>Settings.Designer.cs</LastGenOutput>

diff --git a/DemoApp/MainWindow.xaml b/DemoApp/MainWindow.xaml
@@ -5,14 +5,11 @@
         xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
         xmlns:local="clr-namespace:DemoApp"
         xmlns:oxy="http://oxyplot.org/wpf"
-                xmlns:dg2d="clr-namespace:DataGrid2DLibrary;assembly=DataGrid2DLibrary"
+                xmlns:dg2d="http://gu.se/DataGrid2D"
         mc:Ignorable="d"
         Title="MainWindow" Height="350" Width="525">
 
     <DockPanel>
-        <Grid DockPanel.Dock="Top" Height="50">
-            <Button Height="30" Width="100" Click="Button_Click" Content="Run"/>
-        </Grid>
         <Grid DockPanel.Dock="Bottom" >
 
 

diff --git a/DemoApp/MainWindow.xaml.cs b/DemoApp/MainWindow.xaml.cs
@@ -26,10 +26,5 @@ public MainWindow()
             var x = new MainWindowViewModel();
             this.DataContext = x;
         }
-
-        private void Button_Click(object sender, RoutedEventArgs e)
-        {
-            (this.DataContext as MainWindowViewModel).Run();
-        }
     }
 }
diff --git a/DemoApp/MainWindowViewModel.cs b/DemoApp/MainWindowViewModel.cs
@@ -1,11 +1,6 @@
-using System;
-using System.IO;
-using System.Linq;
-using System.Windows;
+using System;
 using System.Collections.ObjectModel;
 using UnscentedKalmanFilter;
-using MathNet.Numerics.LinearAlgebra.Double;
-using MathNet.Numerics.LinearAlgebra;
 
 namespace DemoApp
 {
@@ -16,101 +11,37 @@ class MainWindowViewModel : INPCBase
         public ObservableCollection<Measurement> Estimates { get; set; }
 
 
-
-        public Matrix<double> Q { get; set; }
-        public Matrix<double> R { get; set; }
-        public FEquation f { get; set; }
-        public HEquation h { get; set; }
-        public Matrix<double> x { get; set; }
-        public Matrix<double> P { get; set; }
-
-
-        private int n; //number of state
-        private double q; //std of process 
-        private double r; //std of measurement
-        private int N;//total dynamic steps
-
         public MainWindowViewModel()
         {
             Measurements = new ObservableCollection<Measurement>();
             Estimates = new ObservableCollection<Measurement>();
-        }
-
-
-        public void Run()
-        {
-            var filter = new UKF(1, 1);
-
-            n = 1;
-            q = 0.05;
-            r = 0.3;
-            N = 100;
-
-            Q = Matrix.Build.Diagonal(n, n, q * q); //covariance of process
-            NotifyChanged("Q");
-            R = Matrix.Build.Dense(1, 1, r * r); //covariance of measurement  
-            f = new FEquation(); //nonlinear state equations
-            h = new HEquation(); //measurement equation
-            x = q * Matrix.Build.Random(1, 1);  //s + q * Matrix.Build.Random(1, 1); //initial state with noise
-            P = Matrix.Build.Diagonal(n, n, 1); //initial state covariance
-
-
-            var xV = Matrix.Build.Dense(n, N, 0); //Estimate
-            var zV = Matrix.Build.Dense(1, N, 0); //measurement
 
+            var filter = new UKF();
+            var N = 100;
 
             for (int k = 1; k < N; k++)
             {
-                Matrix<double> z = ProcessBuilder.SineWave(k, r);
-                //measurments
-
-                Matrix<double>[] x_and_P = filter.Update(f, x, P, h, z, Q, R);                //ukf 
-                x = x_and_P[0];
-                P = x_and_P[1];
-
-                Measurements.Add(new Measurement() { Value = z[0, 0], Time = TimeSpan.FromSeconds(k) });
-                Estimates.Add(new Measurement() { Value = x_and_P[0][0, 0], Time = TimeSpan.FromSeconds(k) ,Variance= x_and_P[1][0, 0] });
-            }
-
-
-
-
-
-        }
-    }
+                double[] z = ProcessBuilder.SineWave(k);
+                filter.Update(z);
+                var state = filter.getState();
+                var covariance = filter.getCovariance();
 
-    public class FEquation : IFunction
-    {
-        public Matrix<double> Process(Matrix<double> x)
-        {
-            return x;
-        }
-    }
-
-    public class HEquation : IFunction
-    {
-        public Matrix<double> Process(Matrix<double> x)
-        {
-            return x;
+                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
     {
-        public static Matrix<double> SineWave(int iteration, double Noise)
-        {
-            return Matrix.Build.Dense(1, 1, Math.Sin(iteration * 3.14 * 5 / 180)).Add(Matrix.Build.Random(1, 1).Multiply(Noise));
-
+        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;
@@ -131,8 +62,6 @@ public double Variance {
         public double UpperDeviation { get;private set; }
         public double LowerDeviation{ get; private set; }
     }
-
-
 }
 
 

diff --git a/DemoApp/packages.config b/DemoApp/packages.config
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<packages>
+  <package id="Gu.Wpf.DataGrid2D" version="0.2.1.0" targetFramework="net452" />
+  <package id="OxyPlot.Core" version="1.0.0" targetFramework="net452" />
+  <package id="OxyPlot.Wpf" version="1.0.0" targetFramework="net452" />
+</packages>
diff --git a/Example/Program.cs b/Example/Program.cs
@@ -16,63 +16,36 @@ class Program
     {
         static void Main(string[] args)
         {
-            var filter = new UKF(1, 1);
+            var filter = new UKF();
 
-            var n = 1; //number of state
-            var q = 0.05; //std of process 
-            var r = 0.3; //std of measurement
-            var Q = Matrix.Build.Diagonal(n, n, q * q); //covariance of process
-            var R = Matrix.Build.Dense(1, 1, r * r); //covariance of measurement  
-            var f = new FEquation(); //nonlinear state equations
-            var h = new HEquation(); //measurement equation
-            var x = q * Matrix.Build.Random(1, 1);  //s + q * Matrix.Build.Random(1, 1); //initial state with noise
-            var P = Matrix.Build.Diagonal(n, n, 1); //initial state covraiance
-            var N = 100; //total dynamic steps
+            List<double> measurements = new List<double>();
+            List<double> states = new List<double>();
+
+            Random rnd = new Random();
 
-            var xV = Matrix.Build.Dense(n, N, 0); //estmate
-            var zV = Matrix.Build.Dense(1, N, 0); //measurement
-
-            for (int k = 1; k < N; k++)
+            for (int k = 0; k < 100; k++)
             {
-                var z = Matrix.Build.Dense(1,1,Math.Sin(k*3.14*5/180)).Add(Matrix.Build.Random(1, 1).Multiply(r)); //measurments
-                zV.SetSubMatrix(0, k, z);                                        //save measurment
-                var x_and_P = filter.Update(f, x, P, h, z, Q, R);                //ukf 
-                x = x_and_P[0];
-                P = x_and_P[1];
-                xV.SetColumn(k, x.Column(0).ToArray());                          //save estimate
+                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]);
             }
 
             GraphPane myPane = new GraphPane(new RectangleF(0, 0, 3200, 2400), "Unscented Kalman Filter", "number", "measurement");
-            PointPairList list_zV = new PointPairList();
-            PointPairList list_xV = new PointPairList();
-            for (int i = 0; i < zV.ColumnCount; i++)
+            PointPairList measurementsPairs = new PointPairList();
+            PointPairList statesPairs = new PointPairList();
+            for (int i = 0; i < measurements.Count; i++)
             {
-                list_zV.Add(i, zV[0, i]);
-                list_xV.Add(i, xV[0, i]);
+                measurementsPairs.Add(i, measurements[i]);
+                statesPairs.Add(i, states[i]);
             }
-            myPane.AddCurve("measurement", list_zV, Color.Red, SymbolType.Circle);
-            myPane.AddCurve("estimate", list_xV, Color.Green, SymbolType.XCross);
+            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);
         }
     }
-
-    public class FEquation : IFunction
-    {
-        public Matrix<double> Process(Matrix<double> x)
-        {
-            return x;
-        }
-    }
-
-    public class HEquation : IFunction
-    {
-        public Matrix<double> Process(Matrix<double> x)
-        {
-            return x;
-        }
-    }
 }
diff --git a/UnscentedKalmanFilter.sln b/UnscentedKalmanFilter.sln
@@ -1,12 +1,14 @@
 
 Microsoft Visual Studio Solution File, Format Version 12.00
-# Visual Studio 2013
-VisualStudioVersion = 12.0.31101.0
+# Visual Studio 15
+VisualStudioVersion = 15.0.26228.4
 MinimumVisualStudioVersion = 10.0.40219.1
 Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "UnscentedKalmanFilter", "UnscentedKalmanFilter\UnscentedKalmanFilter.csproj", "{FADAFD6C-9B9D-444C-B991-398AE5C67712}"
 EndProject
 Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Example", "Example\Example.csproj", "{18445F38-685F-4A9C-BB11-4BB6B18AF296}"
 EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "DemoApp", "DemoApp\DemoApp.csproj", "{4511AC7F-5C7C-4102-B9AB-7A67B472AD3C}"
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|Any CPU = Debug|Any CPU
@@ -21,6 +23,10 @@ Global
 		{18445F38-685F-4A9C-BB11-4BB6B18AF296}.Debug|Any CPU.Build.0 = Debug|Any CPU
 		{18445F38-685F-4A9C-BB11-4BB6B18AF296}.Release|Any CPU.ActiveCfg = Release|Any CPU
 		{18445F38-685F-4A9C-BB11-4BB6B18AF296}.Release|Any CPU.Build.0 = Release|Any CPU
+		{4511AC7F-5C7C-4102-B9AB-7A67B472AD3C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{4511AC7F-5C7C-4102-B9AB-7A67B472AD3C}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{4511AC7F-5C7C-4102-B9AB-7A67B472AD3C}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{4511AC7F-5C7C-4102-B9AB-7A67B472AD3C}.Release|Any CPU.Build.0 = Release|Any CPU
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE

diff --git a/UnscentedKalmanFilter/IFunction.cs b/UnscentedKalmanFilter/IFunction.cs