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] }); } }
static void Main(string[] args) { var filter = new UKF(1, 1); 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 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++) { 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 } 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++) { list_zV.Add(i, zV[0, i]); list_xV.Add(i, xV[0, i]); } myPane.AddCurve("measurement", list_zV, Color.Red, SymbolType.Circle); myPane.AddCurve("estimate", list_xV, 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); }
/// <summary> /// UnscentedKalmanFilter 对一维数据进行处理 /// </summary> /// <param name="data">使用引用传值方式修改data</param> public static void UnscentedKalmanFilterMethod(List <double> data) { var filter = new UKF(); for (int i = 0; i < data.Count; i++) { filter.Update(new[] { data[i] }); // filter.Update(new[] { data1[i], data2[i] }); //filter.Update(new[] { data1[i], data2[i], data3[i] }); try { data[i] = filter.getState()[0]; } catch { System.Diagnostics.Debug.WriteLine("UnscentedKalmanFilterMethod:filter index 超出"); } } }
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 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] }); } }
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]); } GraphPane myPane = new GraphPane(new RectangleF(0, 0, 3200, 2400), "Unscented Kalman Filter", "number", "measurement"); 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); }